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ABSTRACT 


The  main  problem  addressed  by  this  research  is  to  find  an  alternative  to  the  use  of  large 
and/or  expensive  equipment  required  by  conventional  navigation  systems  to  accurately 
determine  the  position  of  an  Autonomous  Underwater  Vehicle  (AUV)  during  all  phases  of 
an  underwater  search  or  mapping  mission. 

The  approach  taken  was  to  advance  an  existing  integrated  navigation  system  prototype 
which  combines  Global  Positioning  System  (GPS),  Inertial  Measurement  Unit  (IMU), 
water  speed,  and  heading  information  using  Kalman  filtering  techniques.  The  hardware 
and  software  architecture  of  the  prototype  system  were  advanced  to  a  level  such  that  it  is 
completely  self-contained  in  a  relatively  small,  lightweight  package  capable  of  on-board 
processing  of  sensor  data  and  outputting  updated  position  fixes  at  a  rate  of  10  Hz;  an 
improvement  from  the  5  Hz  rate  delivered  by  the  prototype.  The  major  changes  to  the 
preceding  prototype  implemented  by  this  research  were  to  install  an  on-board  processor  to 
locally  process  sensor  outputs,  and  improve  upon  the  analog  filter  and  voltage  regulation 
circuitry. 

Preliminary  test  results  indicate  the  newly  designed  SANS  provides  a  100% 
performance  improvement  over  the  previous  prototype.  It  now  delivers  a  10  Hz  update  rate, 
and  increased  accuracy  due  to  the  improved  analog  filter  and  the  higher  sampling  rate 
provided  by  the  processor. 
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I.  INTRODUCTION 

A.  BACKGROUND 

An  Autonomous  Underwater  Vehicle  ( AUV)  can  be  capable  of  numerous  missions  both  overt 
and  clandestine.  Such  vehicles  have  been  used  for  inspection,  mine  counter-measures,  survey, 
observation,  etc.  [Yuh  95].  Recent  research  trends  in  underwater  robotics  have  emphasized 
minimizing  the  need  for  human  interaction  by  increasing  the  autonomy  of  such  vehicles. 

The  NPS  'Phoenix' AUV  is  an  experimental  vehicle  designed  primarily  for  research  in  support 
of  shallow-water  mine  countermeasures  and  coastal  environmental  monitoring  [Healey  93,  95, 
Brutzman  96].  In  [Kwak  93],  an  approach  is  described  for  determining  the  position  of  submerged 
detected  objects  by  executing  a  "pop-up"  maneuver  to  obtain  a  GPS  fix,  and  then  extrapolating  this 
fix  backwards  to  the  submerged  object  location  using  recorded  inertial  data.  As  explained  in  [Kwak 
93],  navigation  accuracy  during  such  a  surfacing  maneuver  is  strongly  enhanced  by  the  use  of 
accurate  depth  information  available  from  low-cost  pressure  cells.  However,  this  form  of  "aided" 
inertial  navigation  [Brown  92],  is  not  applicable  to  a  surfaced  AUV.  Inertial  navigation  is  not 
needed  in  circumstances  where  continuous  reliable  reception  of  GPS  satellite  signals  is  possible. 
However,  this  does  not  apply  to  AUVs,  unless  perhaps  they  are  fitted  with  a  mast  to  extend  a  GPS 
antenna  above  the  effects  of  wave  action.  Such  a  mast  is  not  an  attractive  option  for  military 
operations,  and  in  any  event  may  be  mechanically  difficult. 

In  efforts  to  overcome  the  problem  of  intermittent  GPS  satellite  tracking  for  surfaced  (or 
cruising  near  the  surface)  AUV  navigation,  an  experimental  system,  using  a  low-cost  strapped- 
down  inertial  measurement  unit  (IMU),  has  been  designed  to  enable  inertial  navigation  between 
GPS  fixes.  This  system  is  well  suited  for  pop-up  navigation,  so  finding  a  means  of  navigating  near 
the  surface  provides  a  complete  solution  to  the  overall  navigation  problem  associated  with 
transiting  an  AUV  to  a  shallow  water  work  site,  recording  the  position  of  detected  submerged 
objects,  and  then  returning  to  a  recovery  site  where  stored  mission  data  can  be  uploaded  [McGhee 
95]. 

Many  of  the  missions  of  the  Phoenix  class  of  vehicles  can  be  separated  into  two  distinct 
phases:  transit  and  search.  After  being  launched  from  an  aircraft,  submarine,  or  surface  vessel,  such 
an  AUV  would  conduct  the  transit  phase  of  the  mission  in  order  to  arrive  at  the  search  area.  After 


the  search  phase,  the  AUV  would  transit  back  to  a  recovery  position.  Neither  of  these  transit  phases 
require  as  high  a  degree  of  navigation  accuracy  as  the  search  phase.  Once  established  in  the  mission 
area,  the  Phoenix  would  enter  the  search  phase  which  might  include  missions  such  as  mine- 
hunting,  mapping,  or  environmental  data  collection.  Typically,  the  search  phase  would  require 
more  precise  navigation  which  could  be  provided  by  more  frequent  GPS  fixes  or  by  using 
Differential  GPS  (DGPS)  or  post-processing,  if  available.  Both  mission  phases  may  involve 
waypoint  steering  and  obstacle  avoidance. 

One  of  the  most  important  and  difficult  aspects  of  an  AUV  mission  is  navigation.  It  is 
important  that  the  navigation  system  be  robust  if  the  AUV  is  to  be  capable  of  a  wide  variety  of 
missions.  In  order  to  achieve  such  robustness,  the  AUV  should  be  capable  of  navigating  with  the 
Global  Positioning  System  (GPS)  and/or  an  Inertial  Navigation  System  (INS).  The  GPS  is  capable 
of  highly  accurate  positioning  when  the  AUV  is  surfaced,  while  an  INS  can  be  used  for  submerged 
navigation.  In  order  to  ensure  accurate  navigation  for  the  various  missions,  the  GPS  and  INS 
components  can  be  combined.  A  favorable  analysis  of  this  type  of  navigation  system  was 
conducted  in  [McKeon  92].  The  hardware  and  software  architecture  required  for  a  typical  mapping 
scenario  was  evaluated  in  [Norton  94]. 

[Bachmann  95]  made  the  architecture  evaluated  in  [Norton  94]  a  reality,  and  subsequently 
developed  the  first  working  prototype  of  the  proposed  Shallow-water  AUV  Navigation  System 
(SANS).  With  the  prototype  SANS  having  achieved  favorable  results  in  open-water,  at-sea  test 
trials,  the  research  reported  in  this  thesis  advances  the  SANS  to  another  level  of  maturity,  making 
it  now  ready  for  direct  application  to  a  real-world  AUV. 

B.    RESEARCH  QUESTIONS 

This  thesis  will  examine  the  following  research  topics: 

-  Evaluate  the  hardware  and  software  architecture  of  the  GPS/INS  prototype  SANS. 

-  Evaluate  the  feasibility  of  an  AUV  accurately  navigating  from  point  to  point  using  GPS/INS 
while  conducting  open-ocean  transit. 

-  Develop  a  hardware  configuration  which  will  enable  the  GPS/INS  SANS  to  be  housed  in 
one  small,  self-contained  package. 


C.  SCOPE,  LIMITATIONS  AND  ASSUMPTIONS 

This  thesis  reports  the  findings  of  the  fifth  year  of  research  in  an  ongoing  research  project.  The 
scope  of  this  thesis  is  to  investigate  the  feasibility  of  an  AUV  navigating  from  point  to  point  using 
a  combination  of  GPS/INS.  The  requirements  for  a  SANS  described  by  [Kwak  93]  which  impact 
this  project  are: 

-  Low  power  consumption.  Operation  from  an  appropriately  sized  external  battery  pack  for  12 
hours  is  desirable. 

-  Limited  exposure  time.  The  amount  of  time  that  the  GPS  antenna  is  exposed  in  the  search 
phase  should  be  as  short  as  possible.  Up  to  30  seconds  of  exposure  is  allowed,  but  time  between 
exposures  should  be  maximized. 

-  Maintain  clandestine  operation.  The  GPS  antenna  should  present  a  very  small  cross  section 
when  exposed  and  should  not  extend  more  than  a  few  inches  above  the  surface  of  the  water. 

-  Maximize  accuracy.  During  the  search  phase  of  the  mission,  system  accuracy  of  10  meters 
or  better  is  required  with  postprocessing,  both  submerged  and  surfaced. 

-  Total  volume  not  to  exceed  120  cubic  inches.  Elongated,  streamlined  packaging  is  preferred. 
For  the  purposes  of  this  research,  DGPS  will  be  used  as  ground  truth  data  (without 

postprocessing)  for  determining  appropriate  Kalman  filter  gains.  However,  some  real-world 
scenarios  will  only  utilize  the  noncorrected  GPS  signal  for  real-time  mission  navigation  and  may 
require  further  tuning  of  Kalman  filter  coefficients. 

D.  ORGANIZATION  OF  THESIS 

The  purpose  of  this  thesis  is  to  present  the  development  of  a  system  meeting  the  mission 
requirements  of  the  SANS.  The  term  AUV  is  understood  to  include  any  small  underwater  vehicle 
(including  human  divers)  which  can  easily  carry  such  a  compact  device.  The  term  "towfish"  refers 
to  the  test  vehicle  (depicted  in  Figure  2.2)  used  to  evaluate  the  SANS  during  at-sea  testing. 

Chapter  II  reviews  previous  work  on  this  project  as  well  as  previous  work  on  GPS  navigation 
and  AUV  submerged  navigation. 

Chapter  HI  provides  an  evaluation  of  the  prototype  SANS  in  the  form  of  both  a  time  domain 
and  frequency  domain  analysis  of  the  IMU  sensor  output  signals. 


Chapter  IY  is  a  detailed  description  of  the  hardware  currently  in  use  for  this  portion  of  the 
project. 

Chapter  V  is  a  detailed  description  of  the  software  changes,  additions,  and  updates  made  to 
support  this  portion  of  the  project.  The  chapter  includes  a  description  of  the  C++  code  required  for 
future  towfish  experiments.  This  description  provides  an  explanation  of  the  class  and  object 
hierarchy  used,  as  well  as  an  outline  of  the  major  functions  that  were  added  as  a  result  of  this 
research. 

Chapter  VI  is  a  description  of  the  experiment  design  and  analysis  of  the  experimental  results. 

Chapter  VII  presents  the  thesis  conclusions  and  provides  recommendations  for  future 
research. 


II.  SURVEY  OF  RELATED  WORK 

A.  INTRODUCTION 

Autonomous  Underwater  Vehicles  (AUVs)  have  the  potential  to  be  used  in  an  efficient  and 
cost  effective  manner  in  a  variety  of  missions  involving  military  and  non-military  applications. 
One  of  the  most  important  aspects  of  an  AUV  mission  is  the  ability  to  navigate  accurately.  Many 
possible  missions,  such  as  mine-hunting,  require  a  high  degree  of  navigation  accuracy.  This  chap- 
ter will  discuss  some  of  the  solutions  for  navigating  an  AUV. 

In  general  there  are  two  categories  of  navigation  systems:  those  that  are  based  on  external 
signals  and  those  that  are  based  on  sensors.  External-signal-based  navigation  systems  such  as, 
Loran,  Omega,  and  GPS  are  only  able  to  determine  position  while  the  signal  receiver  is  exposed 
to  the  signal.  Loran  and  Omega  are  relatively  inaccurate  compared  to  GPS.  While  Loran  covers 
almost  the  entire  northern  hemisphere,  it  has  almost  no  coverage  in  the  southern  hemisphere 
[Bowditch  84].  GPS  on  the  other  hand  is  capable  of  world-wide  coverage  with  a  high  degree  of 
navigational  accuracy.  [Logsdon  92] 

Sensor-based  navigation  can  be  implemented  as  a  self-contained  unit  which  can  be  composed 
of  various  types  of  equipment  such  as  IMUs,  acoustic  transponders,  or  geophysical  map 
comparison.  Each  of  these  components  has  its  disadvantages.  Acoustic  transponders  must  be  pre- 
deployed  at  precisely  known  locations  and  may  require  costly  maintenance.  Geophysical  map 
interrogation  requires  a  precise  bottom  contour  map  previously  stored  in  the  AUVs  computer. 
IMU-based  navigation  is  prone  to  sensor  drift,  which  if  left  uncorrected,  can  become  very  large. 

B.  GPS  NAVIGATION 

The  Navigation  Satellite  Timing  and  Ranging  (NAVSTAR)  Global  Positioning  System  (GPS) 
is  a  space-based  radio  positioning,  navigation  and  time-transfer  system  sponsored  by  the  U.S. 
Department  of  Defense  (DoD).  It  was  originally  intended  to  provide  the  military  with  precise  nav- 
igation and  timing  capabilities  [Parkinson  80].  The  system  is  designed  to  provide  24-hour,  all- 
weather  navigation  by  providing  total  earth  coverage  using  24  satellites  in  22,200  km  orbits  that 

are  inclined  55°,  with  12  hour  periods.  The  satellites  broadcast  two  L-band  frequencies:  LI 
(1575.4  MHz)  and  L2  (1227.6  MHz).  Navigation  and  system  data,  predicted  satellite  position 
(ephemeris)  information,  atmospheric  propagation  correction  data,  satellite  clock  error  informa- 


tion,  and  satellite  health  data  are  all  superimposed  on  these  two  carrier  frequencies  [Logsdon  92, 
Wooden  85]. 

There  are  two  different  navigation  services  available  from  the  GPS  satellites  depending  on  the 
type  of  receiver  being  used:  the  Standard  Positioning  Service  (SPS)  and  the  Precise  Positioning 
Service  (PPS).  The  SPS  is  achieved  by  receiving  the  LI  carrier  signal  which  is  broadcast  with  an 
intentional  inaccuracy  called  Selective  Availability  (SA).  SA  limits  world-wide  navigation  to  100 
m  horizontal  accuracy  with  a  95%  confidence  level  [Logsdon  92].  The  PPS  is  limited  to  U.S.  and 
allied  military,  and  specific  non-military  uses  that  are  in  the  national  interest.  Access  to  PPS  is 
restricted  by  use  of  special  cryptographic  equipment.  PPS  provides  the  highest  stand  alone 
accuracy:  15  m  Spherical  Error  Probable  (SEP),  a  velocity  accuracy  of  0.1  m/sec,  and  a  timing 
accuracy  of  better  than  100  nanoseconds  [Logsdon  92,  Wooden  85]. 

In  order  to  take  full  advantage  of  GPS  precision  without  having  access  to  cryptographic 
equipment,  civilian  customers  have  determined  a  way  to  improve  the  accuracy  of  the  SPS.  The 
most  common  way  to  work  around  the  inaccuracies  of  the  SPS  is  Differential  GPS  (DGPS),  which 
may  be  used  in  real-time  or  during  post-processing.  DGPS  is  a  method  which  allows  highly 
accurate  information  to  be  obtained  from  GPS  without  the  cryptographic  equipment  required  for 
access  to  the  P-code  of  PPS.  The  idea  behind  DGPS  is  to  survey  a  receiver  at  a  stationary  site,  allow 
the  stationary  site  to  determine  the  difference  between  its  actual  position  and  its  GPS  position,  and 
broadcast  the  pseudorange  corrections  to  any  DGPS  capable  receivers.  Real-time  differential 
processing  can  reduce  the  typical  100  m  accuracy  of  the  SPS  to  2-4  m  regardless  of  the  status  of 
SA  [Logsdon  92].  In  the  case  of  post-processing,  it  is  possible  to  have  the  AUV  record  the  raw  PPS 
or  SPS  GPS  information  for  later  comparison  to  a  known  geographical  site.  Precise  post- 
processing procedures  can  be  used  to  reconstruct  extremely  accurate  positioning  information, 
typically  in  the  submeter  range.  Table  1  shows  a  comparison  of  expected  GPS  accuracies. 


POSITIONING  SERVICE 

PPS  (m) 

SPS  (m) 

Non-Differential 

16 

100 

Differential 

2-4 

2-4 

Table  1:  Expected  RMS  GPS  accuracy  levels  [Logsdon  92] 


As  GPS  technology  has  matured,  the  size  and  cost  of  GPS  receivers  has  decreased  drastically. 
Not  only  is  miniaturization  improving,  but  GPS  receivers  have  maintained  or  increased  in 
performance  capability.  Since  as  early  as  1992,  the  GPS  industry  has  been  able  to  produce 
receivers  that  are  essentially  a  single  printed  circuit  board.  [Souen  92]  reports  that  the  Furuno  GPS 
receiver  module  LGN-72  is  an  eight-channel  receiver  implemented  on  a  single  printed  circuit 
board  measuring  100  mm  x  70  mm  x  20  mm  and  requiring  only  2  W  of  power. 

There  is  currently  however,  a  performance  trade-off  associated  with  the  miniaturization  of 
GPS  receivers.  Trimble  currently  offers  the  PC  Card  1 10  GPS  receiver  in  the  form  of  a  Personal 
Memory  Card  International  (PCMCIA)  interface.  This  credit  card  sized  device  simply  slides  into 
any  laptop,  most  palmtops,  or  pen-based  computer  compliant  with  PCMCIA  (release  2.0).  This 
miniature  GPS  receiver  is  capable  of  tracking  eight  satellites  using  three  channels.  However, 
because  it  does  not  have  an  allocated  channel  for  each  of  the  eight  satellites  it's  capable  of  tracking, 
it  does  not  use  a  continuous  tracking  scheme,  which  degrades  its  acquisition  time  performance.  In 
order  to  reduce  the  size  of  the  receiver,  manufacturers  are  reducing  the  number  of  channels  on  the 
receiver.  In  this  configuration,  GPS  receivers  are  called  "sequencing"  receivers  [Logsdon  92]. 
Sequencing  receivers  utilize  a  time-sharing  technique  to  "dwell"  on  each  satellite  for  a  brief 
interval  before  switching  to  the  next  satellite  in  the  sequence.  Sequencing  receivers  have  a  typical 
acquisition  time  of  about  2  minutes.  Continuous  tracking  GPS  receivers  have  a  typical  acquisition 
time  of  about  30  seconds,  however,  they  are  less  compact  in  size  as  they  have  more  receiver 
channels.  Given  this  trade-off  relationship  between  size  and  performance,  the  choice  of  GPS 
receiver  must  be  made  with  the  particular  application  in  mind.  If  the  application  is  not  so  dynamic 
(i.e.,  mobile  navigation),  a  sequencing  receiver  would  offer  an  adequate  compromise.  However, 
if  the  application  requires  a  short  time  to  initial  acquisition,  the  most  viable  option  is  the  continuous 
tracking  receiver. 

Given  the  level  of  miniaturization  and  performance  along  with  its  excellent  accuracy,  GPS  is 
an  obvious  choice  for  AUV  navigation.  One  manner  of  using  GPS  to  locate  an  AUV  is  to  place 
buoys  with  GPS  receivers  at  appropriate  locations.  These  buoys  would  translate  the  GPS  signal  and 
retransmit  an  underwater  acoustic  signal.  The  AUV  would  determine  its  position  via  ranging  and 
position  fixes  to  the  buoys.  [Youngberg  91]  suggests  that  the  GPS  antenna,  receiver,  processing 
and  control  subsystem,  acoustic  transmitter,  battery  power,  and  homing  beacon  could  all  be 
contained  in  a  buoy  measuring  123  mm  diameter  x  910  mm  long  and  weighing  5-15  kg.  A 


simulation  which  showed  the  feasibility  of  this  approach  is  presented  in  [Leu  93].  The  simulation 
consisted  of  several  sonobuoys  spaced  one  kilometer  apart.  Due  to  uncertainties  in  buoy  position 
caused  by  wave  action  and  variations  in  altitude,  the  study  proposed  the  use  of  Kalman  filtering 
techniques  to  combine  the  outputs  of  an  accelerometer  and  DGPS  to  enhance  accuracy.  Each  GPS 
buoy  would  essentially  act  as  a  GPS  satellite  and  broadcast  its  position  via  spread  spectrum  signals 
used  by  the  AUV  for  ranging.  This  technique  would  eliminate  the  requirement  to  predeploy  a 
surveyed  transponder  field. 

Another  possible  method  for  using  GPS  to  determine  the  AUV's  position  is  to  physically 
mount  the  GPS  antenna  and  receiver  onboard  the  AUV.  One  major  concern  would  be  that  the  GPS 
receiver  would  be  unable  to  acquire  satellites  in  a  timely  manner  suitable  to  the  mission  due  to 
splash  effects  on  the  antenna.  [Norton  94]  describes  both  static  and  dynamic  test  results  which 
show  that  a  submersible  system  is  able  to  meet  the  accuracy  and  time  requirements  of  the  mission 
while  being  splashed  by  wave  wash. 

C.  INERTIAL  NAVIGATION 

Inertial  navigation  is  basically  a  complex  method  of  dead  reckoning.  In  its  purest  form  it 
involves  no  outside  references  to  fix  position.  All  position  data  is  calculated  relative  to  a  known 
starting  point.  An  inertial  navigation  system  (INS)  continuously  measures  three  mutually  orthog- 
onal acceleration  components  using  accelerometers.  These  measurements  are  taken  in  short  time 
increments  and  multiplied  by  elapsed  time  in  order  to  determine  an  estimate  of  instantaneous 
velocity.  The  three-dimensional  change  in  position  can  then  be  determined  by  integrating  respec- 
tive velocities  over  time.  [Bachmann  95] 

The  primary  drawback  of  any  INS  is  the  tendency  for  small  sensor  drift  rates  to  accumulate 
errors  over  time.  Without  outside  references  for  correction,  these  errors  grow  relentlessly  and 
eventually  lead  to  large  errors  in  the  estimated  position.  Highly  accurate  inertial  navigation 
systems  can  be  constructed,  but  they  are  large,  costly,  and  complex  [Tuohy  93].  Size  alone  makes 
them  unacceptable  candidates  for  the  SANS.  In  order  to  meet  the  SANS  requirements,  a  low-cost 
INS  can  be  integrated  with  GPS.  GPS  will  provide  the  INS  with  the  periodic  position  fixes 
necessary  to  correct  slowly  building  INS  errors. 

The  acceleration  measurements  required  by  an  INS  can  be  made  by  several  types  of  IMUs. 
These  can  be  divided  into  two  fundamental  categories:  gimbaled  and  strapdown.  Due  to  their  large 


size  and  power  requirements,  gimbaled  systems  are  not  suitable  for  the  SANS.  In  a  strapdown  unit, 
three  mutually  orthogonal  accelerometers  and  three  angular  rate  sensors  are  mounted  parallel  to 
the  three  body  axes  of  the  vehicle.  Changes  in  linear  and  rotational  velocities  are  continuously 
measured.  Strapdown  systems  are  smaller  and  simpler  than  gimbaled  systems,  but  necessitate 
much  larger  computational  capabilities  [Logsdon  92]. 

D.  INTEGRATED  GPS/INS  NAVIGATION 

SPS  could  be  used  to  adequately  perform  both  the  transit  and  search  phases  of  an  AUV  mis- 
sion. During  the  transit  phases,  non-differential  SPS  and  a  magnetic  compass  would  provide  the 
primary  source  of  navigation  data.  In  order  to  utilize  GPS  as  a  meaningful  correction  to  a  low-cost 
INS  system,  periods  between  fixes  during  the  transit  phase  must  not  exceed  the  time  in  which  an 
AUV  could  travel  a  distance  greater  than  the  horizontal  accuracy  of  SPS  (100m).  The  mapping 
phases  of  an  AUV  mission  would  require  the  vehicle  to  maintain  more  accurate  navigational  pic- 
ture both  submerged  and  on  the  surface.  This  would  necessitate  the  use  of  periodic  differentially 
corrected  GPS  information  in  order  to  keep  the  INS  system  accurate  while  submerged.  This  dif- 
ferential correction  could  be  provided  in  real-time  during  overt  missions  along  friendly  shores, 
provided  a  DGPS  reference  signal  is  available,  or  during  mission  post-processing  following  a 
clandestine  mission. 

Integration  of  GPS  and  INS  into  a  single  system  can  produce  continuously  accurate 
navigational  information  even  when  using  relatively  low-cost  components.  This  integration  not 
only  allows  periodic  reinstallation  of  the  INS  to  correct  accumulated  errors  but  can  also  (with  the 
aid  of  Kalman  filtering  techniques)  improve  the  performance  of  the  INS  between  fixes.  Filtering 
the  acceleration  data  with  additional  sensor  information  such  as  water  speed  and  heading  will 
further  improve  the  quality  of  the  integrated  system.  Overall,  an  integrated  system  will  provide 
improved  reliability,  smaller  navigation  errors  and  superior  survivability  [Logsdon  92]. 

Kalman  filtering  is  a  method  of  combining  all  available  sensor  data  regardless  of  their 
precision  to  estimate  the  current  posture  of  a  vehicle  [Cox  90].  The  filter  is  actually  a  data- 
processing  algorithm  which  minimizes  the  error  of  this  estimate  statistically  using  currently 
available  sensor  data  and  prior  knowledge  of  system  characteristics.  Each  piece  of  data  is  weighted 
based  upon  the  expected  accuracy  of  the  measurement  it  represents.  In  a  complementary  filter, 
low-frequency  data,  which  is  trusted  over  the  long  term,  and  high-frequency  data,  which  is  trusted 


only  in  the  short  term,  are  used  to  "complement"  each  other  providing  a  much  better  estimate  than 
either  can  alone  [Brown  92]. 

[Bachmann  95]  demonstrated  the  use  of  this  complementary  filter  technique  by  combining  the 
low-frequency  data  of  the  accelerometers  and  compass  with  high-frequency  angular  rate  and 
heading  information.  Intermediate  position  results  were  obtained  by  integrating  high-frequency 
water- speed  data.  GPS  data  was  used  to  reinitialize  the  system  each  time  a  fix  was  obtained  and 
develop  an  error  bias,  expressed  as  an  apparent  current,  to  correct  the  system  between  fixes.  The 
concept  of  using  the  relatively  inexpensive  IMU  with  limited  accuracy  coupled  with  differentially- 
corrected  GPS  has  proven  to  be  a  viable  solution  to  the  challenge  of  shallow-water  AUV 
navigation  [Bachmann  95]. 

E.  AUV  SUBMERGED  NAVIGATION 

There  are  many  techniques  available  for  submerged  navigation,  including  dead  reckoning, 
inertial,  electromagnetic  and  acoustic  navigation.  With  acoustic  navigation,  time  of  arrival  and 
direction  of  propagation  of  acoustic  waves  are  the  two  principal  measurements  made.  A  wide 
variety  of  acoustic  navigation  systems  have  been  developed  for  underwater  vehicle  use.  They  are 
typically  divided  into  long,  short,  and  ultrashort  baseline  systems  (LBL,  SBL,  and  USBL).  All 
involve  the  use  of  an  array  of  acoustic  beacons  or  receivers  whose  positions  must  be  known  to  an 
accuracy  better  than  the  desired  vehicle  localization  accuracy  [Tuohy  93].  Unfortunately,  most 
acoustic  navigation  systems  require  major  expeditions  for  their  accurate  set-up  and  periodic  main- 
tenance. This  makes  them  expensive  and  in  many  ways  reduces  the  level  of  autonomy  achievable 
by  an  AUV.  Also,  acoustic  navigation  methods  are  affected  by  changes  in  the  speed  of  sound  in 
the  ocean  and  suffer  from  refraction  and  multipath  propagation  problems  in  restricted  shallow 
water  coastal  and  ice-covered  areas  [Tuohy  93]. 

There  are  various  other  ways  of  determining  a  vehicle's  velocity  and  position  while 
submerged  without  the  aid  of  external  signals.  An  AUV  could  use  Doppler  sonar  to  determine 
velocity.  Charge  Coupled  Device  (CCD)  cameras,  laser  scanning,  or  variations  in  the  earth's 
magnetic  field  can  also  aid  in  determining  position  [Bergem  93].  Position  could  also  be  estimated 
by  the  double  integration  of  acceleration  as  sensed  by  an  Inertial  Measurement  Unit  (EMU). 

Unless  an  AUV  has  access  to  outside  references,  it  will  not  be  able  to  refer  to  external  signals 
while  submerged.  If  this  is  the  case,  then  the  system  must  rely  on  some  sort  of  dead  reckoning. 
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Modern  dead  reckoning  systems  typically  use  magnetic  or  gyroscopic  heading  sensors  and  a 
bottom  or  water-locked  velocity  sensor  [Grose  92].  The  main  problem  is  that  the  presence  of  an 
ocean  current  will  add  a  velocity  component  to  the  vehicle  which  is  not  detected  by  the  speed  log. 
In  the  vicinity  of  the  shore,  ocean  currents  can  exceed  two  knots  [Tuohy  93].  Using  dead  reckoning 
with  currents  which  are  relatively  large  in  relation  to  the  typical  4-6  knot  speed  of  an  AUV  can 
produce  extremely  inaccurate  results  [Tuohy  93]. 

There  are  many  techniques  for  measuring  accelerations  and  angular  rates.  These  include  using 
ring  laser  and  fiber  optic  gyros,  rotating  mass  gyros,  vibratory  rate  sensors,  and  high  performance 
IMUs.  Inertial  grade  EvlUs  typically  contain  three  angular  rate  sensors,  three  precision  linear 
accelerometers  and  a  three-axis  magnetometer.  The  acceleration  measurements  required  by  an 
Inertial  Navigation  System  (INS)  can  be  made  by  several  types  of  IMUs.  Again,  these  can  be 
divided  into  two  fundamental  categories:  gimbaled  and  strapdown.  All  of  these  sensors  are  subject 
to  drift  errors  which  relentlessly  increase  with  time.  High  quality  sensors  are  subject  to  less  drift 
but  can  cost  up  to  $100,000  [Tuohy  93],  making  them  unattractive  for  small  AUVs. 

[McKeon  92]  proposes  a  combination  of  GPS  and  INS  to  allow  an  AUV  to  determine  position 
information.  While  submerged,  the  AUV  uses  a  low-cost  inertial  navigation  system.  However, 
when  on  the  surface  the  vehicle  has  access  to  GPS  information.  GPS/INS  information  could  be 
combined  with  a  Kalman  filter  techniques  to  reduce  the  errors  during  the  next  dive  sequence  as 
simulated  in  [Nagengast  92]  and  demonstrated  in  [McGhee  95]. 

F.  THE  PREVIOUS  PROTOTYPE 

[Bachmann  95]  describes  in  detail  the  previous  hardware  and  software  configurations  of  the 
SANS.  For  the  benefit  of  the  reader,  Figures  2.1,  2.2,  and  2.3  are  given  again  in  this  thesis  to  aid 
in  discussing  the  previous  prototype.  Figure  2. 1  shows  a  block  diagram  of  the  hardware  assem- 
bled for  at-sea  testing  of  the  SANS  system.  As  seen  in  Figure  2.1,  the  system  relied  on  an  exter- 
nal 386  computer  for  processing  sensor  data,  a  modem  connection  for  transmitting  data  packets  to 
the  towing  vessel,  and  a  coax  cable  to  receive  GPS  data  from  the  towed  vehicle  GPS  antenna.  The 
output  of  the  A/D  converter  was  being  fed  to  the  data-logging  computer  via  an  RS-232  connec- 
tion, and  the  GPS  and  DGPS  receivers  were  physically  located  on  the  towing  vessel.  The  hard- 
ware configuration  for  this  research  has  been  changed  considerably,  and  will  be  presented  in  a 
subsequent  chapter. 
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Figure  2.1:  Towfish  Experiment  Hardware  Configuration 
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Figure  2.2:  SANS  and  Towfish  Components  [Bachmann  95] 
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Figure  2.3:  SANS  Software  Objects  and  Data  Flow  [Bachmann  95] 

Figure  2.2  presents  a  photograph  of  the  major  components  of  the  previous  prototype.  Figure 
2.3  shows  the  SANS  (as  previously  configured)  software  objects  and  the  types  of  data  passed  from 
one  to  another.  In  its  current  configuration,  the  INS  object  no  longer  receives  data  from  the 
sampler  object,  but  rather  from  processor  registers.  Again,  changes  to  the  SANS  software 
configuration  will  be  presented  in  more  detail  in  a  subsequent  chapter  of  this  thesis. 
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G.  SUMMARY 

The  above  survey  has  shown  that  there  are  many  ways  to  overcome  the  challenges  associated 
with  AUV  navigation.  The  choices  range  from  simple  dead  reckoning  to  systems  which  use 
acoustic  information  from  floating  or  stationary  transponders,  to  complex  systems  which  use 
sophisticated  BVIUs  and  GPS  receivers  combined  with  Kalman  filtering  techniques. 

Many  AUV  missions  could  be  accomplished  using  an  integrated  navigation  system  combining 
GPS  and  INS.  Similar  systems  in  other  applications  have  been  demonstrated  to  have  superior  GPS 
signal  acquisition  and  reacquisition  performance  whenever  loss  of  lock  occurs.  This  results  in 
improved  survivability  in  hostile  environments  and  smaller  navigation  errors.  This  research 
continues  an  ongoing  experimental  study  pertaining  to  the  development  of  such  a  system  and  the 
associated  problems.  The  current  system  under  evaluation  is  of  small  physical  size  and  relatively 
low  cost.  The  IMU  selected  is  representative  and  has  limited  accuracy,  so  additional  water-speed 
and  magnetic  heading  information  is  required.  This  means  that  accelerometers  are  used  mainly  to 
derive  low  frequency  attitude  information,  and  are  not  utilized  for  velocity  or  position  estimation 
over  long  periods. 

The  availability  of  differential  GPS  in  open-ocean  tests  in  Monterey  Bay  will  allow  the 
experimental  choice  of  navigation  filter  gains  to  accurately  assess  overall  system  performance  in 
a  variety  of  sea  states  and  for  various  operational  scenarios.  Previous  research  on  the  prototype 
SANS  has  produced  test  results  and  qualitative  error  estimates  which  indicate  that  submerged 
navigation  accuracy  comparable  to  GPS  surface  navigation  is  attainable.  The  research  goal  of  this 
thesis  is  to  refine  the  error  estimates  and  the  hardware  configuration  to  allow  more  prolonged 
submerged  navigation,  and  develop  the  SANS  into  a  self  contained  system  capable  of  being 
internally  or  externally  attached  to  any  AUV  and  delivering  regular,  accurate  position  updates. 
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III.  EVALUATION  OF  THE  PROTOTYPE  SANS  HARDWARE 

A.  INTRODUCTION 

At  the  heart  of  the  prototype  SANS  are  the  GPS  receiver,  the  IMU,  and  other  sensor  devices. 
The  GPS  receiver,  compass,  and  water  speed  sensor  have  not,  up  to  this  point  in  the  course  of  this 
research,  presented  any  serious  problems  in  accuracy  or  dependability.  However,  the  IMU  out- 
puts, after  being  fed  through  signal  processing  and  conditioning  circuitry,  are  suspected  sources  of 
navigation  inaccuracies  [Bachmann  95]. 

This  chapter  does  not  provide  any  evaluation  of  the  GPS/DGPS  receivers,  water  speed  sensor, 
or  compass  since  there  is  no  apparent  major  error  contributions  from  these  devices.  It  does,  how- 
ever, provide  an  investigation  into  what  noise  is  present  in  the  real-world  Systron-Donner 
MotionPak  EVIU.  It  also  provides  an  evaluation  of  the  low-pass  filtering  and  conditioning  cir- 
cuitry. Finally,  it  presents  an  evaluation  summary  of  the  prototype  SANS  hardware. 

B.  NOISE  CHARACTERISTICS  OF  THE  MOTIONPAK  INERTIAL  SENSORS  UNIT 

The  Systron-Donner  Model  MP-GCCCQAAB-100  "MotionPak"  inertial  sensor  unit  consists 
of  a  cluster  of  three  accelerometers  and  three  "Gyrochip"  angular  rate  sensors.  The  accelerome- 
ter  specifications  are  shown  in  Table  3.1.  The  angular-rate  sensor  specifications  are  given  in  Table 
3.2. 

With  the  IMU  placed  on  a  stable  test  bench,  polariod  photographs  were  taken  of  an  oscillo- 
scope screen  display  of  each  sensor  output.  Figure  3.1  depicts  the  respective  noise  characteristics 
in  each  of  the  EVIU  sensor  outputs.  This  analysis  gives  hints  there  is  broadband,  "white"  noise  in 
the  accelerometer  sensors,  while  there  is  a  275  Hz  sinusoidal  signal  present  in  the  angular-rate 
sensors.  Figures  3.2  and  3.3  depict  the  results  of  a  power  spectrum  analysis  of  the  accelerometer 
and  angular-rate  sensor  outputs.  Figure  3.2  shows  the  power  spectrum  between  8  Hz  and  100 
KHz  as  well  as  between  8  Hz  and  50  Hz.  The  spectrum  analyzer  used  for  these  tests  has  its  own 
power  spectrum  which  was  sufficiently  below  the  sensor  signals  by  8  Hz.  Thus  8  Hz  was  chosen 
as  the  start  frequency.  As  these  plots  show,  with  a  reference  level  of  -20  dB  (the  reference  level  is 
depicted  as  the  top-most  horizontal  index  line),  and  using  the  value  of  10  db/Div,  the  signal  has  a 
maximum  value  of  -60  dB  below  50  Hz.  The  signal  energy  drops  off  above  this  point  and 
becomes  relatively  flat  in  the  vicinity  of  100  KHz.  Figure  3.2  shows  that  the  noise  in  the  acceler- 
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Parameter 

Unit 

x-axis 

y-axis 

z-axis 

Range 

g 

1 

1 

2 

Scale  Factor 

V/g 

7.469 

7.478 

3.727 

Scale  Factor  Temp. 
Coefficient 

%/deg  C 

0.001 

-0.002 

-0.001 

Bias 

mg 

-2.447 

4.570 

-0.586 

Bias  Temp.  Coefficient 

jig/degC 

-47 

-66 

-48 

Sensitivity 

Hg 

10 

10 

10 

Bandwidth 

Hz 

797 

757 

901 

Output  Impedance 

Ohms 

2464 

2494 

1177 

Table  3.1:  MotionPak  Accelerometer  Sensor  Specifications  [Systron-Donner  94] 

Parameter 

Unit 

x-axis 

y-axis 

z-axis 

Range 

deg/sec 

50 

50 

50 

Scale  Factor 

mV/deg/sec 

50.151 

49.906 

50.242 

Scale  Factor  Temp. 
Coefficient 

%/deg  C 

0.03 

0.03 

0.03 

Bias 

deg/sec 

-0.06 

0.23 

0.23 

Bias  Temp.  Coefficient 

deg/sec  P-P 

3 

3 

3 

Sensitivity 

deg/sec 

0.002 

0.002 

0.002 

Alignment 

degrees 

0.26 

0.41 

0.34 

Noise 

deg/sec/  JJTz 

0.008 

0.009 

0.008 

Bandwidth  (to  -90  deg 
phase) 

Hz 

70 

71 

71 

Table  3.2:  MotionPak  Angular-Rate  Sensor  Specifications  [Systron-Donner  94] 
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Figure  3.1:  Systron-Donner  IMU  Sensor  Noise  Characteristics 
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ometers  is  in  fact  broadband  white  noise,  since  it  shows  a  power  spectrum  with  energy  across  a 
wide  range  of  frequencies.  The  white  noise  is  most  likely  thermal  and  low-level  EMI  noise.  Fig- 
ure 3.3  shows  there  is  more  "color"  in  the  angular-rate  sensor  noise.  Mainly,  there  appears  to  be 
harmonic  noise  between  1.5  KHz  and  300  KHz  and  several  high-power  noise  peaks  at  100,  200, 
and  275  Hz.  The  peak  at  275  Hz  is  most  likely  harmonic  noise  from  the  fundamental  tuning-fork 
in  the  rate  sensors.  [Matthews  95]  describes  an  investigation  of  signal  noise  in  this  same  Systron- 
Donner  MotionPak  IMU.  [Matthews  95]  found  a  strong  peak  at  275  Hz,  and  attributed  this  noise 
to  the  internal  tuning-fork  oscillator  in  the  sensor  itself. 

The  accelerometer  sensor  outputs  depicted  in  Figure  3.1  show  a  DC  bias  of  -21  mV  and  125 
mV  respectively.  These  DC  biases  are  most  likely  caused  by  a  combination  of  electronic  sensor 
bias  and  slight  tilts  in  the  test  bench  surface.  For  the  purpose  of  this  evaluation,  these  DC  biases 
are  ignored.  Furthermore,  the  software  as  described  in  [Bachmann  95],  uses  bias  correction  fac- 
tors that  essentially  negate  any  sensor  bias  affects  on  navigation  accuracy. 
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Figure  3.2:  Power  Spectrum  of  Accelerometer  Sensors 


20 


10  dS/&IV  «ftN£E  -10.0   dS»  -*!*1! 


StftST   8>«J  NX 

KB*  300   Ks  VB*  i   KK* 


ST    2.4    SCO 


Figure  3.3:  Power  Spectrum  of  Angular-rate  Sensors 


C.  THE  SIGNAL  PROCCESSING  AND  CONDITIONING  CIRCUITRY 

The  signal  processing  and  conditioning  circuitry,  described  in  detail  in  [Schubert  95],  low-pass 
filters  the  IMU  outputs  at  a  cutoff  frequency  of  10  Hz  using  an  active,  anti-aliasing,  two-pole 
Sallen-Key  Bessel  filter  design.  This  circuit  further  converts  the  dual-ended  output  swing  of  the 
IMU  to  a  single-ended  0-5  volts. 

Figure  3.4  depicts  the  frequency  response  of  the  low-pass  filter.  Generally,  this  analysis  con- 
firms that  the  -3dB  frequency  is  about  where  it's  supposed  to  be,  and  the  response  does  in  fact 
rolloff  at  the  expected  -40dB/decade. 
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Figure  3.4:  Frequency  Response  of  the  Low-pass  Filter 
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For  the  SANS  application,  in  order  for  any  sensor  noise  to  not  degrade  the  accuracy  or  resolu- 
tion of  the  measured  signal,  the  energy  in  the  filtered  signal  above  10  Hz  must  be  attenuated  down 
below: 

20  log  -L  =  -HdB 
2 

As  seen  in  Figure  3.4,  the  low-pass  filter  is  not  successfully  able  to  attenuate  signal  noise 

energy   in  the  stop-band  below  approximately  1  KHz  (the  center  horizontal  index  line  is  at  -70 

dB).  The  results  of  this  filter  short-fall  can  be  seen  by  looking  at  the  power  spectrum  of  the  accel- 

erometer  and  angular-rate  sensor  outputs  after  low-pass  filtering.  Figure  3.5  and  Figure  3.6  depict 

the  power  spectrum  of  the  filtered  accelerometer  and  angular-rate  sensors  respectively. 


Figure  3.5:  Power  Spectrum  of  the  Filtered  Accelerometer  Sensor  Output 
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Figure  3.6:  Power  Spectrum  of  the  Filtered  Angular-rate  Sensor  Output 
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This  short-fall  is  more  apparent  in  the  angular-rate  sensor  outputs  than  those  of  the  filtered 
accelerometer  outputs.  The  low-pass  filter  appears  to  be  doing  an  acceptable  job  in  attenuating 
the  noise  energy  in  the  accelerometers.  However,  the  low-pass  filter  is  not  fully  attenuating  the 
noise  energy  in  the  angular-rate  sensors  to  a  level  below  the  resolution  of  the  least  significant  bit 
(LSB).  Specifically,  there  are  noise  peaks  at  30,  100,  and  275  Hz  that  reach  -60dB.  Clearly,  these 
high  energy  noise  peaks  are  affecting  the  LSB.  By  noticing  that  these  noise  peaks  represent  a 
12dB  over-power  above  the  required  noise  floor,  and  solving  the  following  equation  for  N: 

201og-L  =  -12dB 

2N 

N  =  1.99 
it's  apparent  that  this  noise  energy  is  actually  affecting  the  two  least  significant  bits  of  the  mea- 
sured angular-rate  signal. 

During  system  development,  it  was  discovered  that  the  x-angular-rate  signal  being  input  to  the 
A/D  had  apparent  slow  growth  behavior.  Figure  3.7  shows  the  results  of  sampling  the  x-angular- 
rate  sensor  once  every  60  seconds  for  a  period  of  4  hours  after  starting  the  SANS  from  an  initially 
"cold"  state. 
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Figure  3.7:  Sampled  X-Angular-Rate  Over  a  4-Hour  Period 
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The  system  hardware  configuration  used  to  collect  the  data  shown  in  Figure  3.7  included  the 
the  LPF  circuitry  described  in  [Bachmann  95].  This  growth  in  the  signal  turned  out  to  be  a  signif- 
icant discovery  possibly  explaining  some  of  the  inaccuracy  the  SANS  had  been  experiencing  dur- 
ing previous  testing.  It  was  at  first  decided  that  this  growth  was  due  to  heating  effects  in  the  IMU. 
However,  after  replacing  the  LPF  circuitry  with  a  newly  designed  circuit  using  commercially  pro- 
vided filters  (this  new  filter  circuit  is  described  later  in  Chapter  IV),  the  problem  went  away. 
Therefore,  the  growth  error  in  the  x-angular-rate  signal  was  attributed  to  heating  effects  on  the  cir- 
cuit components  conditioning  the  x-angular-rate  signal  in  the  LPF  circuitry  used  in  the  prototype 
SANS. 

D.  SUMMARY 

This  analysis  lends  further  explanation  to  the  results  obtained  in  [McGhee  95],  which  shows 
the  angular-rate  output  fluctuating  as  much  as  4.88  mV;  the  three  least  significant  bits  of  the  12-bit 
measured  signal.  Some  of  this  fluctuation  could  be  attributed  to  any  of  the  causes  stated  in 
[McGhee  95],  but  this  analysis  gives  qualitative  evidence  that  the  two  least  significant  bits  of  the 
measured  angular-rate  data  were  in  error  due  to  sensor  noise. 
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IV.  SYSTEM  HARDWARE  CONFIGURATION 

A.  INTRODUCTION 

Figure  4. 1  presents  a  block  diagram  for  the  hardware  making  up  the  redesigned  SANS.  Figure 
4.2  presents  a  photograph  of  the  SANS  components  fully  assembled  into  their  testing  configura- 
tion. The  project  box  in  which  the  components  are  currently  mounted  is  not  intended  to  be  the 
final  resting  place  for  these  components.  A  more  permanent,  water-tight,  streamlined  housing  is 
currently  in  development.  In  fact,  by  using  this  particular  off-the-shelf  project  box,  and  by  itera- 
tively  installing,  testing,  changing,  and  then  reinstalling  the  components,  an  optimum  space-effi- 
cient configuration  has  been  achieved,  which,  in  turn,  has  driven  the  design  of  the  final  housing. 

This  configuration  is  significantly  different  from  that  presented  in  [Bachmann  95]  for  the  pre- 
vious prototype.  Mainly,  the  SANS  components  are  no  longer  separated;  all  its  components  are 
physically  located  in  one,  self-contained  package.  In  fact,  when  joined  with  its  accompanying 
power  source  (a  12  VDC  battery),  it  is  capable  of  being  strapped-down  to  a  testing  turntable,  or 
inserted  into  the  towfish  (with  slight  towfish  modifications)  described  in  [Bachmann  95].  In  its 
current  configuration,  the  SANS  has  its  processor  and  GPS/DGPS  components  "onboard,"  thus 
no  longer  requiring  the  transfer  of  sensor  data  via  modem  to  an  external  processor  or  GPS/DGPS 
receiver  as  was  shown  in  [Bachmann  95].  In  order  to  maintain  the  ability  for  human  monitoring 
and  interaction  during  the  course  of  an  experiment,  the  SANS's  processor  is  linked  with  an  exter- 
nal processor  via  a  DOS  TCP/IP  network  environment.  This  external  processor's  only  function  is 
to  maintain  a  remote  control  session  with  the  SANS  processor  and  receive  its  attitude  and  position 
updates.  Contrary  to  the  original  SANS  proof  of  concept  design  presented  in  [Bachmann  95],  the 
SANS  now  maintains  the  capability  to  on-board-process  its  own  data  in  order  to  maintain  its  gen- 
eral capability  to  interface  with  any  other  higher-level  processor  (via  a  network),  regardless  of  the 
application. 

B.  HARDWARE  DESCRIPTION 

1.  Computer 

The  on-board  processor  is  an  Extremely  Small  Package  (E.S.P.)  Cyrix  486SLC  DX2  50  MHz 

computer.  This  computer,  pictured  in  Figure  4.3,  is  specifically  designed  to  offer  off-the-shelf 
PC-compatible  solutions  in  space  and/or  power  constrained  environments.  Together,  the  E.S.P. 
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Figure  4.1:  Block  Diagram  of  the  Redesigned  SANS  Hardware  Configuration 

Processor  Module  and  its  accompanying  modules  provide  a  small,  low-power  system  that  does 
not  sacrifice  system  performance  compared  to  a  standard,  desk-top  type  system  [MAXUS  95]. 
This  particular  E.S.P  computer  possesses  a  total  of  eight  modules  which  perform  various  system 
tasks.  A  brief  description  of  the  tasks  these  various  modules  perform  in  the  SANS  is  as  follows: 

a.  486SLC  DX2  CPU  Module 

Besides  providing  the  processing  capability,  the  CPU  Module  provides  the  interface  for  a 
standard  keyboard,  the  Rash  PROM  containing  the  system  BIOS,  and  memory  and  bus  controller 
logic  [MAXUS  95]. 
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Figure  4.2:  SANS  Hardware  Configuration 
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Figure  4.3:  E.S.P.  486SLC  DX2  50  MHz  Computer 

b.  DC-DC  Power  Module 

Provides  for  all  the  system  power  requirements  up  to  a  maximum  35W  total  output.  It 
accepts  an  unregulated  12  VDC  and  provides  the  required  +5,  +12,  -12,  and  -28  VDC  to  power 
various  system  components  and  optional  peripherals  (i.e.,  external  floppy/hard  drive).  [MAXUS 
95] 

c.  VGA  Adapter  Module 

Provides  the  interface  to  operate  an  external  VGA  monitor. 

d.  PC  I/O  Module 

Provides  for  2-Serial  ports  and  1 -Parallel  I/O  port. 

e.  PCMCIA  Module 

Provides  two  type- EI  PCMCIA  sockets  which  conform  to  PCMCIA  Release  2.01  standard 
[MAXUS  951.  These  two  ports  can  be  used  for  a  variety  of  compatible  devices  (i.e.,  Ethernet 
Adapter,  Modem,  GPS  Receiver,  etc.).  For  instance,  this  module  can  accept  SRAM  cards  which 
can  contain  bootable  system  files  or  simply  be  used  to  provide  storage  media.  This  module  was 
included  in  the  current  design  to  provide  additional  secondary  storage  in  the  form  of  PCMCIA 
SRAM  cards,  as  well  as  to  enable  future  expandability. 
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f.  Ethernet  Module 

Provides  the  SANS  with  an  external  ethernet  interface. 

g.  Analog  to  Digital  (A/D)  Module 

Provides  8  differential  or  16  single-ended  input  channels  at  12-bit  resolution.  It  features  a 
single-channel  maximum  sampling  rate  of  333  KHz,  and  an  input  range  from  +/-  1.25mV  to  +/- 
10V  [MAXUS  95].  The  A/D  module  provides  a  34-pin  external  connector  (J3)  to  which  develop- 
ers can  connect  their  input  signals.  In  its  current  configuration,  the  A/D  module  samples  only  8  of 
the  available  16  single-ended  channels.  Table  4.1  shows  the  current  pinout  of  connector  J3  on 
the  A/D  connector  board. 


Signal 

Pin 

Pin 

Signal 

GND 

1 

2 

GND 

GND 

3 

4 

IN_8B  unused 

GND 

5 

6 

IN_8A  depth 

GND 

7 

8 

IN_7B  unused 

GND 

9 

10 

IN_7A  water  speed 

GND 

11 

12 

IN_6B  unused 

GND 

13 

14 

IN_6A  z-axis  angular-rate 

GND 

15 

16 

IN_5B  unused 

GND 

17 

18 

IN_5A  y-axis  angular-rate 

GND 

19 

20 

IN_4B  unused 

GND 

21 

22 

IN_4A  x-axis  angular-rate 

GND 

23 

24 

IN_3B  unused 

GND 

25 

26 

IN_3A  z-axis  acceleration 

GND 

27 

28 

IN_2B  unused 

GND 

29 

30 

IN_2A  y-axis  acceleration 

GND 

31 

32 

IN_1B  unused 

GND 

33 

34 

IN_1  A  x-axis  acceleration 

Table  4.1:  Connector  J3  Pinout 
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h.  DRAM  Module 

Provides  for  high-speed  (70ns)  memory  storage  available  in  2,  4,  6,  8,or  16MB  capacities 
[MAXUS  95].  This  module  is  to  the  E.S.P.  as  a  hard  disk  is  to  a  standard  desk-top  PC. 

2.  Inertial  Measuring  Unit 

The  inertial  navigation  component  of  the  SANS  is  provided  by  a  Systron-Donner  Model  MP- 
GCCCQAAB-100  "MotionPak"  inertial  sensing  unit,  pictured  in  Figure  4.4.  This  self-contained 
unit  provides  analog  measurements  in  three  orthogonal  axes  of  both  acceleration  and  angular 
velocity.  It  consists  of  a  cluster  of  three  accelerometers  and  three  "Gyrochip"  angular  rate  sen- 
sors. General  specifications  are  shown  in  Table  4.1.  Accelerometer  specifications  and  angular  rate 
specifications  are  shown  in  Table  3.1  and  Table  3.2  respectively. 


Figure  4.4:  Systron-Donner  Inertial  Measuring  Unit  [Bachmann  95] 


M) 


Parameter 

Units 

Range 

Input  Voltage 

DC  Volts 

+15,-15 

Input  Current 

Amps 

+0.246,  -0.196 

Temp.  Range 

degrees  C 

-40,80 

Weight 

grams 

912 

Temp.  Sensor 

\i  A/deg  k 

1.0 

Table  4.2:  MotionPak  General  Specifications  [Sytron-Donner  94] 

3.  GPS/DGPS  Receiver  Pair 

The  GPS/DGPS  receiver  used  is  the  ONCORE  8-channel  receiver  which  incorporates  an 
imbedded  DGPS  capability  [Oncore  95].  The  receiver  is  capable  of  tracking  up  to  eight  satellites 
simultaneously.  It  can  provide  position  accuracy  of  better  than  25  meters  Spherical  Error  Proba- 
ble (SEP)  without  Selective  Availability  (SA)  and  100  meters  (SEP)  with  SA.  Typical  Time-To- 
First-Fix  (TTFF)  is  18  seconds  with  a  typical  reacquisition  time  of  2.5  seconds  [Oncore  95].  This 
receiver  meets  or  exceeds  the  capabilities  of  the  receiver  described  in  [Norton  94],  which  demon- 
strated that  under  normal  operating  conditions,  a  receiver  of  this  kind,  is  capable  of  meeting  the 
accuracy  and  time  requirements  of  the  SANS  project.  [Norton  94]  also  demonstrated  that  a 
receiver  with  these  qualities  will  perform  well  when  using  an  antenna  that  is  located  on  or  near  the 
sea  surface  as  is  necessary  during  a  clandestine  mission.  Figure  4.5  shows  the  ONCORE  GPS/ 
DGPS  receiver  used  in  the  SANS  project. 


m 


Figure  4.5:  ONCORE  GPS/DGPS  Receiver 
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4.  Low-Pass  Filters 

Based  on  the  analysis  given  in  Chapter  III,  the  2-pole  Bessel  filters  were  replaced  with  new  fil- 
ters which  offer  a  faster  "rolloff  in  the  stopband  (now  80  dB/Dec).  Each  of  the  six  IMU  outputs 
is  filtered  by  a  4-Pole,  active,  anti-aliasing  low-pass  Butterworth  filter  with  a  bandwidth  of  10  Hz. 
The  low-pass  filters  are  model  DP74  and  are  packaged  in  a  standard  16-pin  dual  in-line  package 
(DIP)  [Frequency  Devices  96].  These  filters  feature  low-harmonic  distortion,  come  factory  tuned 
to  a  user-specified  corner  frequency,  require  no  external  components  or  adjustments,  and  operate 
with  a  dynamic  input  voltage  range  from  non-critical  +/-  5V  to  +/-18V  power  supplies  [Frequency 
Devices  96].  To  implement  these  filters  into  the  SANS,  a  double-sided  printed  circuit  board 
(PCB),  shown  in  Figure  4.6,  was  designed  and  machined  to  receive  all  six  filter  DIPs  as  well  as 
three  quad  op-amp  LM324  DIPs  configured  as  voltage-followers  to  provide  input  and  output  cir- 
cuit protection.  Figure  4.7  shows  the  schematic  diagram  for  one  channel  in  this  low-pass  filter 
circuitry. 


Figure  4.6:  The  Double-Sided  Low-Pass  Filter  PCB 
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Figure  4.7:  Schematic  Diagram  for  One  Channel  of  the  Low-pass  Filter  Circuitry 
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5.  DC-DC  Converter 

To  provide  for  the  requisite  +/-15  VDC,  a  DATEL  model  BWR-15/330-D12  DC-DC  Con- 
verter is  used  to  convert  the  unregulated  12  VDC  battery  input  to  regulated  +/-15  VDC  to  power 
the  low-pass  filter  circuits  as  well  as  the  IMU.  This  converter  features  over-current  and  short-cir- 
cuit protection,  a  compact  form-factor,  high  reliability,  a  minimum  efficiency  of  82%,  and 
employs  switching  regulator  technology,  which  minimizes  heat  generation  and  current  usage 
[DATEL  95].  Though  the  DC-DC  converter  ensures  a  low  noise/ripple  in  the  output  signal,  the 
converter  is  augmented  with  additional  capacitors  in  parallel  with  both  the  input  and  output  pins 
of  the  device.  Figure  4.8  shows  the  use  of  these  capacitors  in  the  DC-DC  converter  circuitry. 


CI:  0. 1  uF  Electrolytic  Capacitor 
C2:  100  uF  Electrolytic  Capacitor 


+  Battery               +15  V  Out 
Common 

-Battery                 -15  V  Out 

C2 

CI 

C2 

CI 

Figure  4.8:  DC-DC  Converter  Circuit 

The  value  of  C 1  was  selected  so  as  to  filter  any  high-frequency  radio  frequencies  (RF)  that 
may  get  inducted  into  the  circuit  from  surrounding  components,  and  the  value  of  C2  was  selected 
so  as  to  filter  any  high-frequency  switching  noise  from  the  converter  itself  (the  converter  switches 
at  165  KHz)  [DATEL  95].  This  DC-DC  Converter  is  physically  mounted  on  the  same  PCB  on 
which  the  low-pass  filters  are  mounted.  This  configuration  allows  the  use  of  PCB  "tracks"  to  sup- 
ply the  filter  circuitry  with  +/-15VDC.  This  regulated  +/-15VDC  as  well  as  EMU  sensor  signals 
are  routed  to  and  from  the  PCB  via  a  DB25. 

6.  Ribbon  Cable 

Physically  connecting  the  IMU,  Low-pass  Filters/DC-DC  Converter  PCB,  the  Analog-Digital 
Converter,  input  power,  water  speed  sensor,  and  depth  sensor,  is  a  25-strand  flat  ribbon  cable. 
This  cable  enables  all  system  components  to  be  easily  interconnected.  Table  4.2  gives  the  pinout 
of  this  25-strand  ribbon  cable  as  well  as  all  DB25  connectors  used  in  the  system.. 
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Pin  Number 

Color 

Description 

1 

Black 

IMU  x-axis  acceleration  output 

2 

Gray 

x-axis  acceleration  input  to  A/D 

3 

Blue 

IMU  y-axis  acceleration  output 

4 

Yellow 

IMU  z-axis  acceleration  output 

5 

Red 

z-axis  acceleration  input  to  A/D 

6 

Black 

unused 

7 

Gray 

unused 

8 

Blue 

-15VDC 

9 

Yellow 

GND  (Ground) 

10 

Red 

unused 

11 

Black 

+15VDC 

12 

Gray 

unused 

13 

Blue 

Depth  sensor  input  to  A/D 

14 

White 

y-axis  acceleration  input  to  A/D 

15 

Purple 

IMU  z-axis  angular-rate  output 

16 

Green 

z-axis  angular-rate  input  to  A/D 

17 

Orange 

+12  VDC  Battery  In 

18 

Brown 

Water  speed  sensor  input  to  A/D 

19 

White 

unused 

20 

Purple 

y-axis  angular-rate  input  to  A/D 

21 

Green 

x-axis  angular-rate  input  to  A/D 

22 

Orange 

IMU  x-axis  angular-rate  output 

23 

Brown 

IMU  y-axis  angular-rate  output 

24 

White 

unused 

25 

Purple 

unused 

Table  4.3:  Pinout  of  the  Ribbon  Cable  and  DB25  Connectors 
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7.  Compass 

The  compass  used  in  the  SANS  project  is  a  Precision  Navigation  model  TCM2  Electronic 
Compass  Module.  This  compass  does  not  employ  the  mechanical  gimbal  technology  utilized  in 
the  compass  described  in  [Bachmann  95],  but  rather  employs  a  three-axis  magnetometer  and  a 
high-performance  two-axis  tilt  sensor  in  a  small  form-factor  [TCM2  95].  The  TCM2  compass 
provides  readings  of  not  only  heading,  but  also  pitch,  role,  and  surrounding  magnetic  field 
strength.  The  TCM2  provides  greater  accuracy  by  calibrating  (performed  by  the  user)  for  distor- 
tion fields  in  all  tilt  orientations,  providing  an  alarm  when  local  magnetic  anomalies  are  present, 
and  giving  out-of-range  warnings  when  the  unit  is  being  tilted  too  far  [TCM2  95]. 

8.  Other  Components 

The  water  speed  sensor  and  the  depth  sensor  are  those  described  in  [Bachmann  95]  and  there- 
fore not  depicted  in  Figure  4.2.  The  GPS  antenna  shown  in  Figure  4.2  is  an  active  antenna,  which 
was  selected  for  its  performance  and  low-profile.  Because  the  E.S.P.  Ethernet  module's  output 
media  type  is  AUI,  a  standard  AUI-to-BNC  media  converter  is  employed  to  allow  the  use  of  dura- 
ble RG-58  coax  cable  to  span  the  roughly  100m  distance  required  while  pulling  the  towfish 
behind  a  towing  vessel.  The  GPS/DGPS  Interface  box  is  nothing  more  than  an  adapter  to  inter- 
face the  GPS  receiver  signal  with  the  serial  COM2  port  of  the  E.S.P.  computer. 

C.  SUMMARY 

The  SANS  design  described  in  this  chapter  is  significantly  different  from  that  described  in 
[Bachmann  95].  The  processing  capability,  along  with  the  GPS/DGPS  receiver,  is  now  on-board 
the  SANS,  making  it  completely  self-contained  with  its  only  external  link  being  that  of  a  DOS 
ethernet  environment  to  a  remote  PC.  The  IMU  sensor  data,  after  low-pass  filtering,  along  with 
water  speed  and  depth  data,  are  converted  from  analog  to  digital  with  12-bit  resolution,  and  then 
joined  with  GPS  data  in  the  on-board  computer  which  computes  updated  attitude  and  position 
information  to  be  exported  over  an  ethernet  socket.  The  hardware  for  this  version  of  the  SANS 
was  chosen  to  comply  with  the  requirements  set  forth  in  [Kwak  93].  Though  there  are  many  pos- 
sible choices  of  hardware  for  each  of  the  components  in  Figure  4.2,  trade-offs  between  accuracy, 
size,  power  requirements,  and  cost  must  be  considered.  As  further  advances  in  miniaturization  are 
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made,  accuracy  will  continue  to  increase  while  price  and  size  decrease,  thus  making  it  easier  to 
meet  the  challenges  of  the  SANS  baseline  requirements. 
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V.  SOFTWARE  DEVELOPMENT 

A.  INTRODUCTION 

The  purpose  of  the  SANS  software  remains  the  same  as  that  described  in  [Bachmann  95]:  "to 
utilize  IMU,  heading,  and  water-speed  information  to  implement  an  ENS,  and  then  integrate  this 
with  GPS  information  into  a  single-system  which  can  produce  continually  accurate  navigational 
information  in  real  time."  This  chapter  will  not  re-introduce  the  software  already  adequately  pre- 
sented in  [Bachmann  95],  but  rather  will  present  only  those  changes,  additions  and  updates,  to  the 
software  described  for  use  on  the  previous  prototype  SANS. 

B.  SOFTWARE  DESCRIPTION 

This  implementation  continues  to  use  a  majority  of  the  software  designed  by  [Bachmann  95], 
and  for  the  most  part,  unchanged.  However,  the  changes  in  the  hardware  architecture  have  driven 
subsequent  changes  to  the  software  design  to  enable  its  use  in  the  current  SANS.  Figure  5.1 
shows  the  software  objects  and  data  flow  of  the  current  SANS.  Though  already  previously  stated, 
these  hardware  changes,  along  with  the  subsequent  software  changes  and  additions,  are  again  pre- 
sented in  detail  as  follows: 

1.  Compass  Data 

In  the  SANS  described  by  [Bachmann  95],  the  compass  data  was  included  in  the  packets 
received  via  modem  from  the  towfish.  This  compass  data  was  then  parsed  out  of  this  packet  for 
use.  This  Xmodem  packet  code  is  no  longer  required  and  has  been  removed.  In  the  current  ver- 
sion of  the  SANS,  the  compass  data  is  received  via  the  COM2  serial  port,  and  thus  requires  code 
to  communicate  with  the  serial  port,  as  well  as  code  to  check  the  "checksum"  and  header  of  each 
compass  message  received.  This  change  was  easily  implemented  by  simply  cutting,  pasting,  and 
altering  previous  com  port  and  checksum  code.  This  code  is  provided  in  Appendix  A. 

2.  GPS  Data 

The  code  required  to  process  GPS  information  is  only  slightly  different  from  that  described  by 
[Bachmann  95].  The  only  changes  required  were  driven  by  the  use  of  an  8-channel  receiver  vice 
the  6-channel  GPS  receiver  used  previously.  The  8-channel  receiver  sends  a  longer  message,  thus 
the  only  changes  were  to  adjust  the  message  length  and  the  location  of  the  checksum  character  in 
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the  GPS  message. 

The  code  was  also  modified  to  include  a  differential  check.  Before  the  code  recognizes  a  GPS 
message  as  being  valid,  the  message  must  pass  three  conditions;  1)  A  checksum  check,  2)  The  fix 
must  be  based  on  at  least  4  satellites,  and  3)  The  differential  bit  in  the  message  must  be  set  (i.e., 
the  fix  must  have  the  differential  correction  calculated  into  it).  This  updated  code  is  provided  in 
Appendix  A  as  well. 
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Figure  5.1:  SANS  Software  Objects  and  Data  Flow 
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3.  Inertial  Sensor  Data 

Like  the  compass  data,  the  inertial  sensor  data  was  previously  included  in  the  packets  received 
via  modem  from  the  towfish.  Currently  however,  the  filtered  inertial  data  is  input  directly  to  the 
A/D  converter  module  in  the  on-board  processor.  Therefore,  there  is  additional  code  running  in 
the  SANS  software  system  which  operates  the  A/D  converter  module,  as  well  as  buffers  this  data. 

a.  A2D 

The  A/D  module,  when  delivered,  came  with  C  source  code  to  run  a  demo  of  the  module. 
This  code  was  modified  and  converted  to  C++  to  fit  the  SANS  application  and  its  object  oriented 
design.  The  A2D  class  provides  all  the  requisite  software  operation  of  the  A/D  module  in  the 
E.S.P.  computer.  The  A/D  module  is  completely  controlled  through  software.  Primary  to  this 
control  is  the  manipulation  of  the  A2D  Control  Register  and  the  A2D  Status  Register.  These  reg- 
isters are  manipulated  by  writing  to  and  reading  from  specific  memory  addresses.  Specifically, 
the  A2D  Control  Register  is  at  address  0x108,  and  the  A2D  Status  register  is  at  0x102.  The  A2D 
class  is  designed  such  that  the  user  maintains  some  degree  of  flexibility.  For  instance,  the  user  can 
choose  between  one  of  two  base  addresses,  0x100  or  0x300,  from  which  these  important  A2D 
registers  are  then  created  by  adding  an  offset  (08h  and  02h  respectively)  to  this  base  address. 

Though  the  A2D  class  has  many  member  functions,  the  SANS  software  only  uses  a  few  in 
accomplishing  its  mission.  All  those  member  functions  not  directly  utilized  in  this  particular 
application  are  useful  to  troubleshoot  problems  with  the  A2D  module,  or  allow  a  wide  range  of 
options  to  tailor  its  use  to  a  particular  application.  The  class  code  is  well  commented,  and  easily 
stands  without  any  further  discussion.  However,  for  the  benefit  of  the  reader,  the  following  gen- 
eral discussion  of  how  the  A2D  module  works  in  the  SANS  application  is  deemed  beneficial. 

12 

The  A2D,  as  stated  in  Chapter  IV,  provides  12  bits  of  resolution,  or  2  =  4096  discrete 
quantization  levels.  There  are  two  modes  to  employ  the  A2D  module:  differential  mode,  or  sin- 
gle-ended mode.  The  SANS  application  employs  the  A2D  in  the  single-ended  mode  of  operation. 
In  this  mode,  the  A2D  is  able  to  sample  the  dual-ended  swing  of  the  EVIU  sensor  signals,  and  rep- 
resent these  voltages  as  a  digital  value  in  the  range  0  -  4095.  A  general  A2D  conversion  table  is 
provided  as  Table  5.1  to  further  illustrate  how  the  sensor  voltages  are  mapped  over  to  their  digital 
equivalents. 


39 


Sensor  DC  Voltage 

Converted  Equivalent 

+  10  Volts 

4095 

+5  Volts 

3071 

0  Volts 

2047 

-5  Volts 

1023 

-10  Volts 

0 

Table  5.1:  A2D  DC-to-Digital  Conversion  Mapping 

When  an  A2D  object  is  instantiated,  the  class  constructor  (see  Appendix  A,  A2D.cpp)  sets 
several  default  data  member  values,  and  then  reads  the  A2D  configuration  file  A2D.cfg.  This 
configuration  file  provides  a  simple  manner  in  which  a  user  can  change  the  way  the  A2D  module 
operates  without  having  to  re-compile  the  source  code.  When  the  function  readConfigFile()  is 
called,  it  reads  the  A2D.cfg  file  one  line  at  a  time  and  loads  those  respective  variables  described 
on  each  line  of  this  file  with  those  values  found  on  each  line.  This  prepares  the  software  for  ini- 
tializing the  A2D  hardware.  The  constructor  initializes  the  system  addresses  to  setup  for  A2D 
operation,  then  initializes  the  A2D  hardware  using  those  variables  that  were  loaded  upon  reading 
the  configuration  file.  The  A2D  object  gets  instantiated  when  a  Sampler  object  gets  instantiated 
as  the  A2D  object  "a2dl"  is  a  private  data  member  of  the  Sampler  class. 

The  A2D  module  gets  set  into  operation  by  a  call  to  initSampler().  This  sampler  class 
member  function  executes  a  sequence  of  function  calls  to  A2D  class  member  functions  which  set 
the  A2D  module  into  operation.  Mainly,  they  program  the  sequencer  to  tell  it  which  channels  to 
sample  in  which  order,  reset  the  A2D  First-In-First-Out  (FIFO)  to  enable  it  to  receive  data,  and 
then  toggle  the  trigger  bit  in  the  A2D  Control  Register  from  a  low  to  a  high  which  starts  the  A2D 
into  operation. 

During  SANS  operation,  the  Sampler  class  member  function  readSamples()  is  called 
repeatedly  to  retrieve  inertial  data  from  the  A2D  FIFO.  It  first  checks  to  ensure  that  the  FIFO  is 
not  FULL.  If  the  FIFO  ever  gets  filled  without  being  immediately  emptied,  it  will  continue  to 
push  data  into  the  FIFO.  Since  there  is  no  room  for  additional  data,  all  samples  from  that  point  on 
will  be  lost.  So,  it  is  evident  that  preventing  the  FIFO  from  overflowing  is  paramount  to  SANS 
operation.   If  the  this  check  is  ever  true,  the  SANS  software  will  terminate  execution.  To  prevent 
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FIFO  overflow,  one  need  only  be  mindful  of  the  rate  at  which  the  A2D  is  sampling  its  inputs  and 
be  sure  to  empty  out  the  FIFO  at  the  same  rate  or  faster.  If  the  FIFO  does  have  data  in  it,  this  data 
is  emptied  from  the  FIFO  and  stored  in  a  doubly- subscripted  array  with  8  rows  and  1000  columns 
to  coincide  with  storing  up  to  1000,  8  channel  samples  of  sensor  data.  Figure  5.2  presents  a 
model  of  this  doubly-subscripted  array  and  how  it  stores  the  data. 


x-acc 

x-acc 

y-acc 

y-acc 

z-acc 

z-acc 

x-ang 

x-ang 

y-ang 

y-ang 

z-ang 

z-ang 

waterspeed 

waterspeed 

depth 

depth 

0 

1     .    .     . 

999 

Sample  Number/ Array  Index 

Figure  5.2:  Model  of  the  A2D  Sample  Array 

As  the  samples  are  being  emptied  from  the  FIFO,  the  variable  "timeCounter"  is  incre- 
mented once  for  every  8  samples  that  are  pulled  from  the  FIFO.  This  variable  is  then  multiplied 
by  the  sample  period  to  calculate  the  "deltaT"  or  the  time  between  adjacent  samples.  The  reason- 
ing behind  using  this  type  of  data  structure  to  temporarily  store  the  data  is  to  enable  access  to  a 
history  of  samples  in  order  to  employ  a  digital  filtering  scheme.  In  the  case  of  the  SANS,  it 
employs  a  very  simple  form  of  low-pass  filtering  by  averaging  over  all  the  samples  received  since 
the  last  sample  was  taken  from  this  array. 

C.  SUMMARY 

All  software  additions  and  updates  to  the  SANS  software  were  made  in  keeping  with  object- 
oriented  paradigms.  The  software  was  written  in  Borland  3.1,  C++  for  DOS. 

Since  the  publication  of  [Bachmann  95],  there  have  been  many  significant  as  well  as  minor 
changes/updates  to  the  SANS  software.  The  compass  data  is  now  received  from  a  serial  port  vice 
being  received  via  an  Xmodem  packet.  The  GPS  data  is  still  received  from  a  serial  port,  but  the 
GPS  receiver  is  now  an  8-channel  receiver  instead  of  a  6-channel.  The  inertial,  water  speed,  and 
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depth  data  is  no  longer  being  received  via  an  Xmodem  packet,  but  rather  is  being  input  directly 
into  the  A/D  module  in  the  E.S.R  computer.  Consequently,  all  the  Xmodem  packet  code  is  no 
longer  used  in  the  SANS  software  system.  All  these  hardware  changes  have  driven  software 
changes  that  have  propagated  throughout  the  software.  For  this  reason,  a  complete  copy  of  all 
SANS  software  is  given  as  Appendix  A.  A  further  discussion  of  those  software  updates  incorpo- 
rated into  the  SANS  between  the  publication  of  [Bachmann  95]  and  this  thesis,  is  presented  in 
[Bachmann  96]. 
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VI.  SYSTEM  TESTING 

A.  INTRODUCTION 

This  chapter  presents  the  test  method  and  the  experimental  results  of  testing  used  to  determine 
the  functionality  and  accuracy  of  the  SANS.  Bench  testing  was  performed  to  ensure  the  entire 
system  was  functioning  properly  in  its  current  state.  The  system  was  then  tested  by  conducting 
tilt-table  tests  in  order  to  verify  the  operation  of  the  Inertial  Measurement  Unit  and  its  associated 
filter  software.  It  was  further  tested  by  conducting  a  static  GPS  test  to  test  the  operation  of  the 
positioning  capabilities  of  the  SANS.  Lastly,  it  was  given  a  static  heading  test  to  ensure  proper 
operation  of  the  Kalman  filter  in  determining  heading. 

Figure  6.1  presents  a  data  flow  diagram  of  the  SANS  navigation  filter.  This  diagram  is  pre- 
sented at  this  point  to  provide  a  basis  for  discussion  of  the  following  test  results.  The  reader  is 
referred  to  [Bachmann  95,  96],  and  [McGhee  95]  for  an  in-depth  discussion  of  this  filter.  This 
twelve-state  velocity-aided  navigation  filter  is  implemented  in  the  SANS  software  provided  as 
Appendix  A  and  Appendix  B. 

B.  IMU  TESTS 

The  purpose  of  these  tests  was  to  qualitatively  ensure  that  the  SANS  was  able  to  accurately 
track  changes  in  attitude.  Paramount  to  getting  the  SANS  to  do  this,  is  to  find  a  suitable  value  for 
the  gain  Kl (shown  in  Figure  6.1),  the  biasWght,  sampleWght  (all  in  ESfS.CPP),  and  x/yAc- 
celScale  factors  (in  LOCATION.H).  The  results  presented  in  this  chapter  are  for  the  pitch  axis 
only.  A  similar  process  still  remains  to  be  done  for  the  roll  axis. 

With  the  SANS  mounted  to  the  tilt-table  described  in  [Bachmann  96],  a  series  of  pitch  tests 
were  conducted.  For  these  tests,  the  IMU  outputs  were  sampled  at  a  rate  of  40  Hz.  During  test- 
ing, it  was  noticed  that  the  SANS  produced  update  rates  between  6-10  Hz. 

Tuning  data  for  the  SANS  was  obtained  by  moving  the  SANS  unit  through  pitch  excursions 
within  a  45  degree  range.  The  attitude  as  determined  by  the  SANS  was  then  plotted  and  compared 
with  the  actual  motion  of  the  unit.  Through  this  comparison,  it  was  possible  to  determine  an  initial 
scale  factor.  This  process  was  repeated  several  times  until  the  attitude  determined  by  the  filter 
'matched'  the  true  motion  of  the  unit.  From  this  analysis,  a  rate  sensor  scale  factor  of  4.1  enabled 
the  filter  to  register  a  pitch  of  45  degrees  when  the  SANS  was  pitched  to  that  angle. 
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Figure  6.1:  SANS  Navigation  Filter 

In  order  to  tune  the  filter  for  accurate  operation,  the  rate  sensor  bias  value  must  first  be  deter- 
mined. To  do  this,  the  gain  value  Kl  is  set  to  zero  in  order  to  prevent  accelerometer  data  from 
getting  to  the  first  integrator.  Without  the  accelerometer  data,  only  the  high  frequency  data  from 
the  angular- rate  sensors  is  input  to  the  first  integrator  with  the  estimated  bias.  Taking  the  com- 
manded tilt-table  angles  to  be  truth  then,  any  errors  in  attitude  can  be  attributed  to  the  bias  and 
scale  factor. 

Having  determined  the  initial  scale  factor  to  be  4.1  and  setting  K  1=0.0,  a  series  of  pitch  tests 
were  conducted  in  order  to  determine  the  correct  bias  weights.  In  order  to  obtain  a  starting  point 
for  this  analysis,  and  based  on  similar  testing  discussed  in  [Bachmann  95],  an  initial  bias  value 
(biasWght)  of  0.999  was  chosen  for  the  first  pitch  test.  Figure  6.2  shows  the  results  of  the  first  45 
degree  pitch  excursion  with  Kl  =  0.0,  bias  value  =  0.999,  and  the  scale  factor  set  to  4.1. 
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Figure  6.2:  45  Degree  Pitch  Excursion  w/Kl=0,  biasWght=.999,  scale  factor=  4.1 

The  shape  of  the  plot  in  Figure  6.2  is  determined  by  the  value  of  a  "virtual'  time  constant.  For 
the  sake  of  this  discussion,  this  time  constant  is  T .  The  method  in  which  the  SANS  software 
determines  the  rate  bias  value  can  be  essentially  modeled  as  a  low-pass  filter.  Specifically,  the 
SANS  software  figures  this  rate  bias  according  to  the  model  depicted  in  Figure  6.3.  This  figure 
describes  a  linear  system  where  the  accumulating  rate  bias  is  subtracted  from  each  new  angular- 
rate  value,  multiplied  by  the  sampleWght  ((At)  /% ),  which  is  (1  -  biasWght),  and  then  this 
value  is  added  to  the  rate  bias  sum. 
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Figure  6.3:  Model  of  Rate  Bias  Low-pass  Filter 
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From  linear  system  theory,  in  the  s-domain,  it  can  be  shown  that  the  transfer  function  of  this 
system  is: 

1 

1  +XS 

Assuming  the  10  degree/second  pitch  excursion  to  be  a  unit  step  function,  the  unit  step 
response  of  this  system  is: 

x(t)  =[l-^'AJ 
By  taking  the  first  derivative,  it  can  be  shown  that  the  bias  filter  output  slope  is  1/x.  Using  the 
relation  that  the  sampleWght  (.001  for  the  first  test)  is  equal  to  the  ratio  of  the  time  between  sam- 
ples (at  a  sample  rate  of  7  Hz,  this  gives  a  period  of  0. 142)  and  x ,  it  can  be  shown  that,  for  the  first 
test  depicted  in  Figure  6.2,  x  is  approximately  142  seconds.  That  is, 

0.001  =  —  =  —  =»x  =  142sec 

x         x 

Given  the  known  angular-rate  of  the  pitch  excursion  was  10  degrees/second,  the  bias  filter  output 
slope  can  then  be  calculated  as: 

-—  =  0.07  (deg/sec)/sec 

From  the  plot,  it  appears  to  have  taken  about  4.5  seconds  to  complete  the  45  degree  angular-rate 
excursion.  Multiplying  the  filter  slope  by  the  4.5  seconds  yields  a  resulting  bias  estimate  of 
approximately  0.3  deg/sec.  Again,  from  the  plot,  the  slope  of  the  curve  after  it  reached  45  deg, 
but  before  it  started  its  return  to  zero,  agrees  with  the  calculations. 

In  short,  the  filter  is  behaving  just  as  it  should.  During  the  course  of  the  excursion  to  45 
degrees,  the  bias  filter  accumulates  a  rate  bias  of  0.3  deg/sec.  Since  the  filter  time  constant  is  only 
142  seconds,  and  the  SANS  only  underwent  a  4.5  second  excursion,  it  did  not  have  time  to  correct 
for  this  bias.  Additionally,  since  the  bias  is  subtracted  from  each  new  sample,  the  result  is  the 
negative  sloping  portion  of  the  curve  while  the  SANS  is  at  45  degrees  pitch. 

Obviously,  the  goal  of  this  analysis  is  to  minimize  any  accumulated  rate  bias  while  the  SANS 
is  undergoing  pitch  or  roll  excursions.  To  accomplish  this,  the  value  of  the  time  constant  x  needs 
to  be  long  enough  to  minimize  the  bias  filter  slope,  which  in  turn,  minimizes  the  accumulated  rate 
bias. 
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Figure  6.4:  45  Degree  Pitch  Excursion  w/Kl=0,  biasWght=.9999,  scale  factor=  4.1 

Figure  6.4  shows  the  test  results  after  increasing  the  filter  time  constant  by  a  factor  of  10.  As 
shown  in  the  plot,  the  pitch  values  sloped  off  less  by  about  a  factor  of  10  (as  expected)  during  a 
pitch  excursion  to  45  degrees.  The  decreasing  curve  leading  up  to  the  45  degree  pitch  is  due  to 
not  initializing  the  bias  filter  with  enough  samples.  For  these  tests,  the  filter  was  only  initialized 
with  100  samples,  which  represents  about  15  seconds.  However,  since  the  time  constant  is  much 
larger,  the  filter  should  be  initialized  over  more  samples.  Ideally,  the  filter  should  be  initialized  for 
x  seconds.  The  plot  still  shows  some  bias  effect  as  well  as  the  effects  of  undersampling  (or  slow 
update  rate).  Here,  the  navigation  filter  gets  an  update  just  before  the  tilt-table  stops,  but  the  filter 
still  thinks  it  has  a  rate  bias  and  applies  it  to  the  next  sample,  which  occurs  after  the  tilt-table  has 
stopped. 

Figure  6.5  shows  the  result  of  another  similar  test,  only  with  Kl  =0.1,  biasWght  =  .9999, 
sampleWght  =  .0001,  and  xAccelScale  =4.1.  This  test  re-introduces  the  low-frequency  data 
(accelometer  data)  into  the  integrator.  This  plot  shows  yet  more  accurate  performance,  but  dis- 
plays an  "overshoot"  in  both  directions  of  the  excursion.  This  overshoot  can  be  alleviated  by 
adjusting  the  scale  factor. 
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Figure  6.5:  45  Degree  Pitch  Excursion  w/Kl=0.1,  biasWght=.9999,  scale  factor=  4.1 

Figure  6.6  shows  the  result  of  another  similar  test,  only  with  Kl  =0.1,  biasWght  =  .9999, 
sample Wght  =  .0001,  and  an  adjusted  scale  factor,  xAccelScale  =  3.895.  This  plot  shows  an 
undershoot,  indicating  the  scale  factor  was  too  low.  Additionally,  it  still  shows  a  small  amount  of 
sampling  effect.  This  can  only  be  corrected  by  getting  the  navigation  filter  to  run  faster.  This  can 
be  easily  accomplished  by  adding  a  math  co-processor  to  the  SANS  computer. 


Time  (seconds) 
Figure  6.6:  45  Degree  Pitch  Excursion  w/K  1=0.1,  biasWght=.9999,  scale  factor=  3.895 
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C.  STATIC  GPS  TEST 

The  purpose  of  this  test  was  to  ensure  proper  functioning  of  the  DGPS  and  the  Kalman  filter  in 
calculating  position.  With  the  SANS  stationary,  operating,  and  receiving  differentially  corrected 
GPS  fixes,  position  updates  were  recorded  over  a  30  minute  period  at  a  rate  of  10  Hz.  Figure  6.7 
confirms  proper  operation  of  the  DGPS  hardware  and  the  Kalman  filter  software.  The  position 
updates  show  an  oscillating  behavior  roughly  centered  about  the  origin. 
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Figure  6.7:  Static  GPS  Test  Results 

The  results  shown  in  Figure  6.7  are  well  within  the  10  meter  requirements  outlined  in  [Kwak 
93].  By  looking  at  a  plot  of  the  x  position,  it  was  determined  that  the  greatest  drifts  shown  in  Fig- 
ure 6.7  occurred  early  into  the  test,  and  as  time  went  on,  these  drifts  got  smaller.  This  plot  shows 
how  the  SANS  Kalman  filter  "learns"  as  time  progresses.  In  fact,  this  drift  becomes  smaller  and 
smaller  due  to  the  accumulation  of  apparent  current.  At  the  beginning  of  the  test,  the  apparent 
current  is  small.  Any  error  in  location  is  attributed  to  apparent  current,  so  as  time  goes  on,  the 
magnitude  of  the  apparent  current  grows.  Because  the  apparent  current  velocity  is  added  to  the 
North  &  East  velocity,  the  difference  between  the  INS  determined  position  and  the  GPS  deter- 
mined position  decreases  with  time.  Figure  6.8  shows  a  plot  of  the  magnitude  of  the  apparent  cur- 
rent during  the  same  test  as  that  depicted  in  Figure  6.7.  This  plot  shows  that  the  apparent  current 
starts  out  small,  but  quickly  adjusts  to  a  randomly  varying  value  reflecting  the  accumulation  of  all 
system  errors. 
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Figure  6.8:  Apparent  Current 
D.  STATIC  HEADING  TEST 

The  purpose  of  this  test  was  to  ensure  that  the  heading  calculated  by  the  Kalman  filter  was  in 
fact  continuous  in  behavior.  That  is,  a  rotation  through  North  in  the  clockwise  direction  should  not 
cause  the  heading  value  to  go  back  to  zero,  but  rather  continue  to  increase.  With  the  SANS  sta- 
tionary and  running,  the  compass  was  rotated  several  complete  rotations  in  both  the  clockwise 
and  counterclockwise  directions.  As  Figure  6.9  shows,  the  heading  value  is  in  fact  continuous  in 
nature  as  the  plot  of  heading  does  not  show  any  discontinuity  at  the  zero  degree  crossing  point. 
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Figure  6.9:  Static  Heading  Test  Results 
E.  SUMMARY 

This  chapter  provides  an  explanation  and  results  analysis  of  the  experimental  tests  performed 
on  the  SANS  to  determine  its  proper  functioning  under  the  newly  designed  architecture. 

Dynamic  tilt-table  tests  were  performed  in  order  to  "tune"  the  software  scale  factors,  bias  val- 
ues, and  gain  factor  Kl  used  in  the  Kalman  filter.  These  test  also  confirmed  the  proper  operation 
of  the  SANS  in  general. 

The  DGPS  and  the  associated  software  was  tested  by  conducting  a  static  test  during  which  the 
SANS  remained  stationary  while  getting  position  fixes.  This  test  confirmed  the  proper  operation 
of  the  DGPS  hardware  and  software.  The  behavior  of  the  results  also  indicate  that  the  SANS  soft- 
ware Kalman  filter  is  properly  attributing  its  navigation  errors  to  apparent  current,  as  expected. 

The  compass  and  associated  software  was  tested  by  analyzing  the  heading  output  of  the  SANS 
while  the  compass  was  rotated  through  several  complete  rotations.  The  results  show  a  continuous 
heading  output  from  the  SANS,  that  is,  there  are  no  discontinuous  "jumps"  as  the  compass  rotates 
through  North. 

Though  extensive  testing  of  the  SANS  still  remains,  initial  testing  indicates  qualitatively  that 
the  SANS  does  function  properly.  Based  on  observations  during  this  testing,  the  SANS  shows  an 
improved  performance  over  the  previous  proof  of  concept  prototype.  Mainly,  the  SANS  now  pro- 
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vides  an  improved  update  rate  of  up  to  10  Hz  in  comparison  to  the  5  Hz  of  the  prototype.  This 
increase  in  update  rate  has  reduced  the  effects  of  undersampling  experienced  with  the  prototype 
and  explained  in  [Bachmann  95].  Figure  6.6  provides  insight  to  conclude  the  SANS  is  still  suffer- 
ing from  some  effects  of  undersampling.  This  effect  will  most  likely  be  alleviated  with  the 
planned  addition  of  a  math  co-processor  to  the  E.S.P.  CPU  Module. 
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VII.  CONCLUSIONS  AND  RECOMMENDATIONS  FOR  FUTURE  WORK 

A.  CONCLUSIONS 

The  research  issues  addressed  by  this  thesis  were  1)  Evaluate  the  hardware  architecture  of  the 
prototype  SANS,  2)  Develop  a  hardware  configuration  which  will  enable  the  SANS  to  be  housed 
in  one  small,  self-contained  package,  and  3)  Evaluate  the  feasibility  of  an  AUV  accurately  navi- 
gating from  point  to  point  in  open-ocean  transit  using  this  hardware  configuration. 

The  work  conducted  in  addressing  the  first  of  these  questions  revealed  several  sources  of  nav- 
igation inaccuracy.  The  analog  low-pass  filter  circuitry  used  in  the  prototype  SANS  was  not  ade- 
quately attenuating  the  angular-rate  sensor  noise  below  the  resolution  of  the  least  significant  bit  of 
the  12-bit  A/D,  and  was  over-designed  to  require  high  current,  thus  generated  a  great  deal  of  heat. 
It  was  also  discovered  that  circuit  components  were  being  adversely  affected  by  heat.  Specifi- 
cally, the  x-angular-rate  signal  displayed  a  significant  growth  behavior  over  time  as  the  circuit 
heated  up.  Both  of  these  problems  were  alleviated  by  employing  higher  quality,  commercially 
available  analog  filters,  and  also  employing  a  switching  DC-DC  converter  in  place  of  the  linear 
regulators  employed  in  the  prototype  SANS.  All  this  circuitry  was  designed  into  a  small  double- 
sided  PCB. 

In  addressing  the  second  of  these  questions,  an  emphasis  on  making  the  SANS  integrated  and 
self-contained  produced  a  configuration  in  which  all  SANS  components  are  physically  packaged 
in  a  project  box  measuring  17.5"  X  6.125"  X  3.0".  In  the  prototype  SANS,  the  EMU,  compass, 
GPS  antenna,  and  water  speed/depth  sensors  were  physically  separated  from  the  computer  and 
DGPS  receiver.  The  newly  designed  SANS  no  longer  requires  the  use  of  the  data  logging  com- 
puter (which  did  the  A/D  conversion  and  assembled  data  packets)  or  the  Motorola  modems.  The 
SANS  now  employs  a  E.S.P.  486  DX2  50  mini  computer  with  an  internal  12-bit  A/D  converter, 
DC-DC  converter  module  (enables  computer  to  be  powered  from  a  12  Volt  battery),  Ethernet 
module  (enables  SANS  to  be  networked  in  a  client/server  environment),  PCMCIA  module  (pro- 
vides SANS  with  expandability),  and  memory  storage  modules.  From  observing  the  SANS  oper- 
ation during  bench  testing,  it  is  evident  the  system  runs  faster  (gives  faster  update  rates)  since  it  is 
no  longer  hindered  by  the  "bottleneck"  induced  by  the  Xmodem  transfer  of  data  employed  by  the 
prototype  SANS. 
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Though  the  last  research  question  was  not  directly  addressed  in  detailed  at-sea  testing,  those 
tests  that  were  conducted  for  this  thesis  qualitatively  indicate  that  the  newly  designed  hardware 
configuration  provides  a  higher  level  of  performance  to  the  SANS.  From  this  result,  it  can  be 
extrapolated  that  the  feasibility  of  an  AUV  accurately  navigating  during  open-ocean  transit,  is  at 
least  as  high  as  that  achieved  with  the  prototype  SANS.  It  is  likely  that  the  newly  designed  SANS 
will  demonstrate  increased  accuracy  in  at-sea  trials. 

B.  RECOMMENDATIONS  FOR  FUTURE  WORK 

There  are  many  ways  to  build  on  the  foundation  this  and  related  research  has  established.  In 
addition  to  comprehensive  at-sea  testing,  and  since  this  project  research  is  dynamic  in  nature, 
there  are  several  other  areas  that  remain  to  be  addressed  as  they  pertain  to  the  SANS.  Before  any 
worthwhile  at-sea  testing  can  be  accomplished,  further  tilt-table/bench  testing  needs  to  be  con- 
ducted. In  order  to  optimize  the  performance  of  the  Kalman  filter  used  for  inertial  navigation, 
testing  needs  to  be  done  in  order  to  better  optimize  the  gains  in  the  Kalman  filter.  By  analyzing 
the  data  from  varied  accelerometer  and  angular-rate  tests,  one  can  better  choose  what  these  gain 
values  need  to  be. 

An  issue  directly  related  to  testing  is  that  of  post-processing.  The  post-processing  code  writ- 
ten to  run  on  the  previous  prototype  SANS  should  be  updated/changed  to  enable  its  use  on  the 
newly  designed  SANS.  Through  post-processing  test  data,  one  can  more  easily  run  and  re-run  the 
tests  in  the  goal  of  more  easily  optimizing  the  Kalman  filter  gains.  The  current  version  of  the 
SANS  software  does  not  save  the  "raw"  sensor  data.  This  should  be  made  possible.  With  the 
help  of  post-processing  code,  one  needs  only  run  a  particular  tilt/bench/cart  test  one  time,  and 
then  take  the  raw  data  back,  analyze  it,  adjust  gains  and  re-run  the  test  in  order  to  optimize  gains. 

There  are  hardware  issues  that  remain  and  are  areas  for  future  work.  The  SANS  uses  a  paddle- 
wheel  type  water  speed  sensor.  This  is  a  rather  crude  and  inaccurate  sensor,  and  should  be 
upgraded  to  one  which  can  deliver  reliable  data  at  the  slower  velocities  under  which  the  towfish 
and  Phoenix  AUV  operate.  Additionally,  in  order  to  conduct  future  at-sea  testing,  the  DGPS 
antenna  currently  used  on  the  SANS  will  have  to  be  upgraded.  In  previous  sea  trials,  the  towfish 
would  become  fowled  by  seaweed.  Under  these  conditions,  the  current  DGPS  antenna  will  not 
survive.  Due  to  difficulties  in  acquiring  software  drivers  for  the  Ethernet  module,  the  network 
connection  between  the  SANS  and  an  external  PC  was  not  established  during  the  course  of  this 
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thesis.  In  order  to  conduct  at-sea  testing,  this  network  connection  is  a  must,  so  work  still  remains 
in  bringing  this  aspect  of  the  SANS  to  fruition. 

Lastly,  the  final  step  in  this  research  will  be  incorporation  of  the  SANS  into  the  NPS  AU V.  In 
this  work,  there  should  not  be  any  significant  changes  to  the  hardware.  However,  there  will  be 
requirements  in  interfacing  this  hardware  with  the  operating  system  running  the  NPS  AUV.  It 
may  be  beneficial  from  both  a  space-saving  aspect  as  well  as  from  the  aspect  of  having  more  com- 
puting power,  to  transfer  all  software  operation  over  to  the  Sun  Voyager  workstation  which  cur- 
rently accomplishes  mission  control  for  the  NPS  AUV.  In  this  case,  the  issues  of  serial  port 
communications,  and  A/D  conversion  will  have  to  be  readdressed  before  this  can  be  successful. 
On  the  other  hand,  there  is  a  distinct  advantage  to  leaving  the  SANS  as  is  and  simply  integrating 
it  into  the  NPS  AUV  as  another  client  in  its  established  client/server  environment.  A  detailed 
study  of  these  alternatives  will  be  required  before  the  best  solution  can  be  developed. 
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APPENDIX  A:  Real  Time  Navigation  Source  Code(C++) 
A.  TOWTYPES.H 

#ifndef  TOETYPES_H 

# define  TOETYPES_H 

#include  <stdio.h> 
#include  <dos.h> 
#include  <time.h> 

#include  "globals.h"    -/ /  Types  used  by  serial  communications  software 

#define  GPSBLOCKSIZE  76//  Size  of  Motorola  @@Ea  position  message 

#define  COMPSIZE  60     //  Max  size  of  compass  message 

#define  biasNumber  10   //  Number  of  packets  used  to  calculate  initial  bias 

#define  ONE_G  32.2185//  One  g  in  feet  per  second 
#define  GRAVITY  32.2185  //In  feet  per  second 

#define  TicksToSecs (x)  ((double)  ((10  *  x)  /  182)) 

typedef  char  ONEBYTE; 
typedef  short  TWOBYTE; 
typedef  long   FOURBYTE; 

typedef  unsigned  char  UNSIGNED_ONEBYTE; 
typedef  unsigned  short  UNSIGNED_TWOBYTE; 
typedef  unsigned  long   UNSIGNED_FOURBYTE; 

//  Holds  lat/long  expressed  in  milseconds 
struct  latLongMilSec  { 

long  latitude; 

long  longitude; 
}; 

//  Holds  a  latitude  or  longitude  expressed  in  hours  minutes  and  degrees 
struct  T_GEODETIC  { 

TWOBYTE  degrees ; 

UNSIGNED_TWOBYTE   minutes; 

double  seconds; 

>; 

//  Holds  a  latitude  and  longitude  expressed  as  T_GEODETICs 
struct  latLongPosition  { 

T_GEODETIC  latitude; 

T_GEODETIC  longitude; 
}; 

//  Holds  a  grid  position 
struct  grid  { 

double  x,y , z ; 
}; 


57 


//  3  X  3  matrix 
struct  matrix  { 

float  element [3 ] [3 ] ; 
}; 

//  3  X  1  matrix  or  vector 
struct  vector  { 

float  element [3 ]; 
}; 


//  Oversize  area  to  hold  a  GPS  message 
typedef  BYTE  GPSdata[2  *  GPSBLOCKSIZE] ; 

//  Defines  a  type  for  holding  compass  messages 
typedef  BYTE  compData[2  *  COMPSIZE] ; 

//  Structure  for  passing  around  various  types  of  INS  information. 
//  The  positions  in  the  sample  field  of  a  stampedSample  structure 


// 

sample 

[0] 

x  acceleration 

// 

sample 

[1] 

y  acceleration 

// 

sample 

[2] 

z  acceleration 

// 

sample 

[3] 

phi 

// 

sample 

[4] 

theta 

// 

sample 

[5] 

psi 

// 

sample 

[6] 

.  water  speed 

// 

sample 

[7] 

heading 

struct  stampedSample  { 
grid  est; 

float  rawSample [8] ; 
double  sample [8]; 
float  deltas- 
float  current ; 

}; 

#endif 


//position  as  estimated  by  the  INS. 
//Original  readings  for  post  processing 
//sampler  converted  sample. 


B.  LOCATION.H 

//Conversion  constants  for  location  of 

//36:35:42.2N  andl21 : 52  :  28 . 7W 

#define  LatToFt  0.10134  //converts  degrees  Latitude  to  ft 

#define  LongToFt  0.08156  //converts  degrees  Longitude  to  ft 

#define  HemisphereConversion  -1  11-1    if  west  of  of  Greenwich 

#define  RADIANMAGVAR  0.261799   //  Local  area  Magnetic  variation  in  radians 

#define  xyAccelLimit  0NE_G//  Max  accell  in  x  and  y  diretion 
#define  zAccelLimit  2  *  0NE_G   //  Max  accel  in  z  direction 
#define  rateLimit   0.872665//  Max  rotational  rate  in  radians 
#define  speedLimit  25.3  //Max  water  speed 
#define  headingLimit  2  *  M_PI 

#define  pScale  1.0  //roll 
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#define  qScale  1.0  //pitch 
#define  rScale  1.0  //yaw 

#define  xAccelScale  1.34   //1.078  //pitch 
#define  yAccelScale  3.895  //roll 
#define  zAccelScale  1.34   //1.038 

#define  pUnits (angular )  (pScale  *  ( ( (angular-2047 . 0 )  /  2047.0  ) 

*  50.0)  *  (M_PI/180.0) ) 

#define  qUnits (angular )  (qScale  *  (( (angular-2047 . 0 )  /  2047.0  ) 

*  50  .0)  *  (M_PI/180.0)  ) 

#define  rUnits (angular )  (rScale  *  (( (angular-2047 . 0 )  /  2047.0  ) 

*  50.0)  *  (M_PI/180.0) ) 

#define  xAccelUnits ( linear )  (xAccelScale  *  ( ( linear-2047 . 0 )  / 

2047.0  )  *  GRAVITY) 
#define  yAccelUnits ( linear )  (yAccelScale  *  (( linear-2047 . 0 )  / 

2047.0  )  *  GRAVITY) 
#define  zAccelUnits ( linear )  (zAccelScale  *  (( linear-2047 . 0 )  / 

2  047.0)  *  (2.0  *  GRAVITY)) 
#define  waterSpeedScale  1.0  //1.827 
#define  depthUnits (depth)  (((depth  -  819.0)  /  (4095.0-819.0)) 

*  180.0) 
#define  waterSpeedUnits (speed)   (waterSpeedScale  *  ((speed  - 

2047.0)  /  2048.0)  *  25.3)  //feet  per  second 

#define  radToDeg ( 180 . 0/M_PI) 
#define  degToRad   (M_PI/180.0) 
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C.  TOWFISH.CPP 

#include  <stdlib.h> 
#include  <stdio.h> 
#include  <string.h> 
#include  <iostream.h> 
#include  <conio.h> 

# include  "toe types. h" 
# include  "nav.h" 

int   breakHandler (void) ; 

void  screenSetUp (void) ; 

void  printPosition  (const  latLongPosition&) ; 

/•it*********************************** 

PROGRAM:   Main 

AUTHOR:    Eric  Bachmann,  Dave  Gay 

DATE:      11  July  1995 

FUNCTION:  Drives  the  navigator  and  its  associated  software.  Counts 

the  positions  and  displays  each  to  the  screen.  Exited  only  when 

control  break  is  entered  at  the  keyboard. 
RETURNS :   0 
CALLED  BY: none 
CALLS:     initializeNavigator  (nav.h) 

navPosit  (nav.h) 

printPosition 

breakHandler 
**+*********************************•******************** 

int 

main  (int  argc,  char  *argv[]) 

{ 

ctrlbrk (breakHandler ) ;   //  trap  all  breaks  to  release  com  ports 
setcbrk(l);     //  turn  break  checking  on  at  all  times 

char  *dataFile,  *scriptFile,  *attitudeFile ; 

switch  (argc)  { 
case  2  : 

scriptFile  =  new  char [strlen (argv [ 1 ])] ; 

strcpy (scriptFile,  argv[l] ); //explicit  script  file  only 

dataFile  =  "data" ; //default  raw  data  file 

attitudeFile  =  "attitude"; 

break; 

case  3 : 

scriptFile  =  new  char [strlen (argv[ 1] )] • 
strcpy (scriptFile,  argv[ 1] ); //explicit  script  file 
dataFile  =  new  char [ strlen (argv [2 ])] ; 
strcpy (dataFile,  argv[2] ) ; //explicit  data  file 
attitudeFile  =  "attitudes- 
break; 
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case  4 : 

scriptFile  =  new  char [strlen (argv[ 1] )] ; 

strcpy (scriptFile,  argv[ 1] ); //explicit  script  file 

dataFile  =  new  char [strlen (argv[2] )] ; 

strcpy (dataFile,  argv[2 ]); //explicit  data  file 

attitudeFile  =  new  char [strlen (argv[3 ])] • 

strcpy (attitudeFile,  argv[3 ]); //explicit  attitude  file 

break; 

default : 

scriptFile  =  "  script "; //default  script  file 
dataFile  =  " rawdata" ;/ /default  raw  data  file 
attitudeFile  =  "attitude"; 


clrscr ( ) ; 

cout  <<  "\nWriting  script  information  to  "  <<  scriptFile; 

cout  <<  "\nWriting  binary  data  to  "  <<  dataFile; 

cout  «    "\nWriting  attitude  data  to  "  <<  attitudeFile  <<   endl ; 

cout  <<  " \nlnitializing  .  .  ."; 

//Instantiate  the  navigator 

navigator  navl (scriptFile,  dataFile,  attitudeFile); 

latLongPosition  currentLocation;  //  Lat/Long  of  most  recent  fix 
Boolean   fixReceived  =  FALSE; //True  if  a  new  fix  was  recieved 
int       fixCount=0;   //  Count  of  navigation  fixes  recieved 

//Initialize  the  navigator 

currentLocation  =  navl . initializeNavigator ( ) ; 

gotoxy (1,6) ; 

cout  <<  "Initialization  Complete ! \n" ; 

cout  <<  "Initial  Position: \n" ; 

//Print  the  initial  position 

cout  <<  "latitude:  "  <<  currentLocation . latitude .degrees  <<  ' : ' 
<<  currentLocation . latitude .minutes  <<  ':' 
<<  currentLocation. latitude . seconds  <<  endl; 
cout  <<  "longitude:  "  <<  currentLocation . longitude .degrees  <<  ' 
<<  currentLocation . longitude .minutes  <<  ':' 
<<  currentLocation . longitude . seconds ; 

screenSetUp ( ) ; 

while  (TRUE)   { 

//  Attempt  to  get  a  fix  from  the  navigator 
fixReceived  =  navl .navPos it (currentLocation) 
if  (fixReceived)  { 

//  New  fix  recieved 

gotoxy (8, 11) ; 

cout  <<  ++fixCount; 
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printPosition (currentLocation) ; 
} 
} 
} 

/*********************************************************************** 

PROGRAM:  printPositon 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Displays  position  to  the  screen 

RETURNS:  void 

CALLED  BY:  mail 

CALLS :      none 
***************************************************************** 

void 

printPosition  (const  latLongPositionSc  posit) 

{ 

gotoxy (11, 14) ; 

cout  <<  posit . latitude .degrees  <<  ':' 

<<  posit . latitude .minutes  <<  ':'  <<  posit . latitude . seconds  <<  endl; 
gotoxy ( 12 , 15 ) ; 
cout  <<  posit . longitude .degrees  <<  ':' 

<<  posit . longitude .minutes  <<  ':'  <<  posit . longitude . seconds  <<  endl; 
} 


/*********************************************************************** 

PROGRAM:  breakHandler 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Cleans  up  com  ports  upon  program  exit. 

RETURNS :  0 

CALLED  BY:  main 

CALLS:      cleanup  (portBank.h) 
*************************************************************************/ 

int 

breakHandler (void) 

{ 

COMports . cleanup ( ) ; 

exit (0) ; 

return  0;       //  keep  the  compiler  happy 

} 
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/*********************************************************************** 

PROGRAM : screenSetup 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 12  May  1996 

FUNCTION: Sets  up  the  output  screen 

RETURNS : 0 

CALLED  BY:   main 

CALLS :       none 
****************************************************** 

void 

screenSetUp (void) 

{ 

gotoxy (4, 11)  ; 

cout  <<  "Fix  " ; 

gotoxy (1, 14) ; 

cout  <<  "Latitude:  "  <<  " \nLongitude :  " ; 

gotoxy (1, 17) ; 

cout  «  "Roll:  "  «  "\nPitch: 

gotoxy (1, 25)  ; 

cout  «  "deltaT:  " ; 

int  col (45) , row(l) ; 


gotoxy ( col , row++ ) ; 
cout  <<  "x  accel : 
gotoxy (col, row++) ; 
cout  «  "y  accel : 
gotoxy ( col , row++ ) ; 
cout  <<  "z  accel: 
gotoxy ( col , row++ ) ; 
cout  «  "phi  dot : 
gotoxy ( col , row++ ) ; 
cout  <<  "theta  dot: 
gotoxy (col , row++ ) ; 
cout  <<  "psi  dot:  " ; 
gotoxy ( col , row++ ) ; 
cout  <<  "water  speed 
gotoxy (col, row++ ) ; 
cout  <<  "heading:  " ; 


col  =  45; 
row  =  12; 

gotoxy (col, row++) ; 
cout  <<  "x:  " ; 
gotoxy ( col , row++ ) ; 
cout  <<  "y :  " ; 
gotoxy  (col, row++ )  ; 
cout  <<  " z  :  " ; 
gotoxy ( col , row++ ) ; 
cout  <<  "phi :  " ; 
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gotoxy ( col , row++ ) ; 
cout  «  "theta:  " ; 
gotoxy (col , row++ ) ; 
cout  <<  "psi :  " ; 

gotoxy (45, 20)  ; 

cout  <<  "Bias  Values"; 

gotoxy (60,20)  ; 

cout  <<  "Current  Values"; 


D.  NAV.H 

#ifndef  NAVIGATOR_H 

ttdefine  NAVIGATOR_H 

#include  <stdio.h> 
#include  <fstream.h> 
#include  <iostream.h> 
# include  <math.h> 

ttinclude  "toetypes.h" 

#include  "gps.h" 

#include  "ins.h" 

#include  "location.h" 

//  Converts  milseconds  to  degrees 

#define  MSECS_TO_DEGREES  (1.0/(1000.0  *  3600.0)) 

/*********************************************************************** 

CLASS :  navigator 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Combines  GPS  and  INS  information  to  return  the  current 

estimated  position. 
******************************************************** 

class  navigator  { 

public : 

//Constructor,  opens  script  and  data  files 

navigator (char  scriptFile[]  =  "navScript "  ,  char  dataFile[]  =  "navData", 

char  attitudeFile [ ]  =  "navatt") :  positionData  (scriptFile) , 

attitudeData(attitudeFile) , elapsedTime (0.0) ,  f ixCount (0) ,  gpsSpeedSum( 0 . 0) 

insSpeedSum (0.0) 

{if  ((rawData  =  f open (dataFile,  "wb" ) )  ==  NULL)  { 

cout  «  "NO  RAW  DATA  RECORD";} 
} 

//Destructor,  closes  script  and  data  files 

-navigator ( )  {positionData .close ( ) ;attitudeData .close ( ) ; f close (rawData) ; } 
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//provides  the  navigator's  best  estimate  of  current  position 
Boolean  navPosit  ( latLongPosition&) ; 

//Initialize  the  navigator 
latLongPosition  initializeNavigator ( )  ; 

private : 

float  elapsedTime;  //  Tracks  time  since  navigator  was  initialized 
int  fixCount;       //  the  number  of  position  fixes  obtained 

double  gpsSpeed,  insSpeed; 
double  gpsSpeedSum,  insSpeedSum; 

INS  ins 1; //INS  object  instance. 
GPS  gpsl;//GPS  object  instance. 

of stream  attitudeData; //  Post  processing  attitude  data, 
ofstream  positionData;   //  Position  script  file 
FILE  *rawData;//  Post  processing  binary  data  file. 

latLongMilSec  origin;  //lat-long  of  navigational  origin 

//Write  position  information  to  script  file 

void  writeScriptPosit ( int ,  latLongMilSeck,  char); 

//Write  an  INS  packet  and  its  timeStamp  to  the  outPut  file 
void  writelnsData (const  stampedSamplek  drPosition); 

//Write  a  GPS  message  to  the  outPut  file, 
void  writeGpsData (const  GPSdata&  satPosition) ; 

//Returns  the  position  in  Miliseconds 
latLongMilSec  getMilSec (const  GPSdata&) ; 

//Convert  position  in  milSec  to  degress,  minutes,  seconds  and  milsec 
latLongPosition  mi lSecToLatLong (const  latLongMilSec&) ; 

//Convert  xy  (grid)  position  to  lat  long 
latLongMilSec  gridToMilSec (const  grid&) ; 

//Converts  lat/long  to  xy  position 

grid  milSecToGrid( const  latLongMilSec&) ; 

//Parses  and  returns  the  time  of  a  GPS  message, 
double  getGpsTime  (const  GPSdataSc  rawMessage); 

//Parses  and  returns  the  velocity  in  fps  of  a  GPS  message. 

double  getGpsVelocity  (const  GPSdataSc  rawMessage); 
}; 
#endif 
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E.  NAV.CPP 

# include    "nav.h" 

#include    "signal. h" 

#define  SIGFPE  8//  Floating  point  exception 

/*********************************************************************** 

PROGRAM:   navPosit 

AUTHOR:    Eric  Bachmann,  Dave  Gay 

DATE:      11  July  199  5 

FUNCTION: Provides  the  navigator's  best  estimate  of  current  position. 

Attempts  to  obtain  GPS  and  INS  position  fixes  from  the  gps 

and  ins  objects  and  copies  the  most  accurate  fix  available 

into  the  input  argument  ' navPosition ' .  Writes  the  raw  position 

fix  data  to  the  output  file  for  post  processing.  Sets  a  return 

flag  to  indicate  whether  a  valid  fix  was  obtained. 

RETURNS:   TRUE,  a  valid  position  fix  is  in  the  variable  'navPosition' . 

FALSE,  otherwise. 

CALLED  BY:towf ish.cpp  (main) 

CALLS:       gpsPosition  (gps.h) 

correctPosition  (ins.h) 

insPosition  (ins.h) 

getMilSec  (nav.h) 

gridToMilSec  (nav.h) 

milSecToGrid  (nav.h) 

milSecToLatLong  (nav.h) 

writeScriptPosit  (nav.h) 
************************************************************* 

void  fpeNavPosit ( int  sig) 

{if  (sig  ==  SIGFPE)  cerr  <<  "floating  point  error  in  navPosit\n" ; } 

Boolean 

navigator :: navPosit  ( latLongPosition&  navPosition) 

{ 

signal  (SIGFPE,  fpeNavPosit); 

GPSdata  satPosition;       //  the  latest  GPS  position 

stampedSample  dr Posit ion;  //  the  latest  INS  position 

latLongMilSec  gpsMilSec;   //  the  latest  GPS  position  in  milseconds 

latLongMilSec  insMilSec;   //  the  latest  INS  position  in  milseconds 

//Attempt  to  get  the  INS  and  GPS  positions 
Boolean  insFlag  =  insl . insPosition (drPosition) ; 
Boolean  gpsFlag  =  gpsl .gpsPosition (satPosition) ; 

//INS  and  GPS  positions  obtained? 
if  ( insFlag  &&  gpsFlag)  { 

gotoxy (20 , 11 ) ; 

cout  <<    "GPS" ; 

//  Write  INS  packet  and  attitude  info  to  an  output  file 

elapsedTime  +=  drPosition .deltaT; 

writelnsData (drPosition) ; 

//Write  GPS  message  to  output  file. 

writeGpsData (satPosition) ; 
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//Parse  position  from  GPS  messsage 
gpsMilSec  =  getMilSec ( satPosition) ; 
//Write  milsec  position  to  script  file 
writeScriptPosit (++f ixCount ,  gpsMilSec,  'G'); 

//Pass  GPS  position  to  INS  object  for  navigation  corrections, 
insl .correctPosition (milSecToGrid (gpsMilSec) ,  getGpsTime (satPosition) ) ; 
//Covert  position  in  mil  sec  to  latitude  and  longitude. 
navPosition  =  milSecToLatLong (gpsMilSec) ; 
return  TRUE; 
} 
else  { 

//Only  INS  position  obtained? 
if  (insFlag)  { 

gotoxy  (20, 11)  ; 

cout  <<  "    " ; 

//  Write  INS  Packet  to  output  file. 

elapsedTime  +=  drPosition .deltaT; 

writelnsData (drPosition) ; 

insMilSec  =  gridToMilSec (drPosition. est ) ; 

//Write  milsec  position  to  script  file 

writeScriptPosit (++fixCount,  insMilSec,  ' I ' ) ; 

navPosition  =  milSecToLatLong ( insMilSec) ; 

insSpeed  =  drPosition . sample [ 6 ]  ; 

return  TRUE; 
} 
else  { 

//  Only  GPS  position  obtained? 
if  (gpsFlag)  { 

gotoxy (20, 11)  ; 

cout  «  "GPS"; 

//  Write  GPS  message  to  output  file. 

writeGpsData ( satPosition) ; 

//Parse  position  from  GPS  messsage 

gpsMilSec  =  getMilSec (satPosition) ; 

//Write  milsec  position  to  script  file 

writeScriptPosit (++fixCount ,  gpsMilSec,  'G'); 

//Pass  GPS  position  to  INS  object  for  navigation  corrections. 

insl . correctPosition (mi lSecToGrid( gpsMilSec) , 

getGpsTime ( satPosition) ) ; 
//Convert  position  in  mil  sec  to  lat/long. 
navPosition  =  milSecToLatLong (getMilSec (satPosition) ) ; 

gpsSpeed  =  getGpsVelocity (satPosition) ; 

gpsSpeedSum  +=  gpsSpeed; 

insSpeedSum  +=  insSpeed; 

gotoxy (58, 9) ; 

cout  <<  gpsSpeed; 

gotoxy (58, 10) ; 

cout  <<  gpsSpeedSum  /  insSpeedSum; 

return  TRUE; 
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else  { 

return  FALSE;  //  No  new  position  available 

} 

} 

} 

} 
/*********************************************************************** 

PROGRAM:     writeScriptPosit 

AUTHOR:      Eric  Bachmann,  Dave  Gay 

DATE:        11  July  1995 

FUNCTION:    Writes  the  fix  number,  the  position  in  milSec  and  the  type 

of  fix  to  the  script  file. 
RETURNS:     void 
CALLED  BY:   navPosit  (nav.cpp) 

initialPosit   (nav.cpp) 

CALLS :       None 
***************************************************** 

void 

navigator : :writeScriptPosit ( int  fixNumber,  latLongMilSec&  posit,  char  fixType) 

{ 

positionData  <<  fixNumber  <<  '  ' 

<<  posit . latitude  <<  '  ' 

<<  posit . longitude  <<  '  ' 

<<  fixType  <<  '  ' 

<<  elapsedTime  <<  endl; 
} 

/*********************************************************************** 

PROGRAM:  writelnsData 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Writes  the  packet  and  the  time  stamp  contained  in  a  stamped 

sample  to  the  out  put  file  for  post  processing. 

RETURNS:  void 

CALLED  BY:  navPosit  (nav.cpp) 

CALLS :        None 
*************************************************************************/ 

void 

navigator : :writeInsData (const  stampedSample&  drPosition) 

{ 

//MUST  ADD  CODE  TO  RECORD  DATA  FOR  POST  PROCESSING  HERE 

//Output  attitude  data  to  a  file 

attitudeData  <<  elapsedTime  <<  '  ' 

«  drPosition .sample [0]  «    '  ' 

«    -1.0  *  drPosition . sample [ 1]  <<  '  ' 

<<  drPosition . sample [2]  «    '     ' 

«    (radToDeg  *  drPosition . sample [ 3 ] )  <<  '  ' 

<<  (radToDeg  *  drPosition . sample [4 ] )  <<  '  ' 

«    (radToDeg  *  drPosition . sample [ 5 ] )  <<  '  ' 

<<  drPosition . sample [ 6]  <<  '  ' 
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/ 


<<  (radToDeg  *  dr Posit  ion . sample [7 ] )  <<  '  ' 
<<  drPosition .current  <<  endl; 

} 
/*********************************************************************** 

PROGRAM:  writeGpsData 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Writes  a  raw  GPS  message  to  a  binary  output  file  for 

post  processing. 

RETURNS:  void 

CALLED  BY:  navPosit  (nav.cpp) 

CALLS :       None 
********************************************************* 

void 

navigator :: writeGpsData (const  GPSdatak  satPosition) 

{ 

for(  int  j  =  0;  j  <  GPSBLOCKSIZE;  j++)  { 
putc (satPosition [j ] ,  rawData) ; 

} 
} 

/*********************************************************************** 

PROGRAM:      init ializeNavigator 

AUTHOR:       Eric  Bachmann,  Dave  Gay 

DATE:         11  July  1995 

FUNCTION:     Obtains  an  initial  GPS  fix  for  use  as  a  navigational  origin  for 

grid  positions  used  by  the  INS  object.  Saves  the  origin  and  passes 

it  to  the  INS  object  in  latLong  form. 

RETURNS :   TRUE 
CALLED  BY:    towfish  (main) 
CALLS:        gpsPosition  (gps.cpp) 

correctPosition  (ins.cpp) 

getMilSec  (nav.cpp) 

milSecToGrid  (nav.cpp) 
*************************************************************•***********/ 

latLongPosition 

navigator: : init ializeNavigator ( ) 

{ 

stampedSample  biasPackets [biasNumber] ; 

GPSdata  satPosition;    //gps  position  message 

//  Loop  until  an  initial  GPS  fix  is  obtained, 
while (! gpsl .gpsPosition (satPosition) )  {/**/} 

//  Write  GPS  message  for  the  grid  origin  to  output  file. 

writeGpsData (satPosition) ; 

//Save  navigational  origin  for  later  grid  position  conversions. 

origin  =  getMilSec (satPosition) ; 

//Write  the  initial  position  to  the  script  file 
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writeScriptPosit ( 0 ,  origin,  '  G'); 

//Pass  time  of  first  GPS  fix  to  INS  object  initialization  routine 

insl . insSetUp (getGpsTime (satPosition) ,  biasPackets) ; 

for  (int  i  =  0 ;  i  <  biasNumber;  i++)  { 
writelnsData (biasPackets [ i ] ) ; 

} 

insSpeed  =  biasPackets [biasNumber  -  1] . sample [ 6] ; 

gpsSpeed  =  getGpsVelocity (satPosition) ; 

//Return  the  initial  position  to  the  caller, 
return  milSecToLatLong (origin) ; 


/*********************************************************************** 

PROGRAM: getMilSec 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  19  9  5 

FUNCTION: Extracts  a  position  in  mili  seconds  from  a  Motorola  (@@Ba) 

position  contained  in  the  input  argument  'rawMessage'  and  returns  it. 

RETURNS: The  latitude  and  longitude  in  milseconds. 

CALLED  BY:    navPosit  (nav.cpp) 

initializeNavigator  (nav.cpp) 

CALLS : none . 
**************************************************** 

latLongMi lSec 

navigator  :: getMilSec  (const  GPSdataSc  rawMessage)  ( 

FOURBYTE  temps4byte; 
latLongMi lSec  position; 

temps4byte  =  rawMessage [ 15] ; 

temps4byte  =  ( temps4byte«8)  +  rawMessage  [  16] 

temps4byte  =  ( temps4byte<<8)  +  rawMessage [ 17 ] 

temps4byte  =  (temps4byte<<8)  +  rawMessage [18] 

position. latitude  -   temps4byte; 

temps4byte  =  rawMessage [ 19 ] ; 

temps4byte  =  ( temps4byte<<8 )  +  rawMessage [20 ] 

temps4byte  =  ( temps4byte<<8 )  +  rawMessage [21] 

temps4byte  =  ( temps4byte<<8 )  +  rawMessage [22] 

position. longitude  =  temps4byte; 

return  position; 
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/*********************************************************+************* 

PROGRAM : mi lSecToLat Long 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Converts  a  position  expressed  in  totally  in  mili  seconds  to 

degress,  minutes,  seconds  and  mili  seconds  and  returns  the  result. 

RETURNS: The  position  in  degress,  minutes,  seconds  and  mili  seconds. 

CALLED  BY:    navPosit  (nav.cpp) 

CALLS :none 
***************************************************** 

latLongPosition 

navigator :: mi lSecToLatLong (const  latLongMilSec&  milSec)  { 

latLongPosition   position; 

double  degrees,  minutes; 

degrees  =  (double) milSec . latitude  *  MSECS_TO_DEGREES ; 

position . latitude .degrees  =  (TWOBYTE) degrees ; 

if (degrees  <  0) 

degrees  =  fabs (degrees) ; 

minutes  =  (degrees  -  (TWOBYTE) degrees )  *  60.0; 

position . latitude .minutes  =  ( TWOBYTE ) minut es ; 

position. latitude . seconds  =  (minutes  -  (TWOBYTE) minutes)  *  60.0; 

degrees  =  ( double) mi lSec . longitude  *  MSECS_TO_DEGREES ; 
position . longitude .degrees  =  (TWOBYTE) degrees ; 

if (degrees  <  0) 

degrees  -    fabs (degrees) ; 

minutes  =  (degrees  -  (TWOBYTE) degrees )  *  60.0; 
position . longitude .minutes  =  (TWOBYTE) minutes ; 
position . longitude . seconds  =  (minutes  -  (TWOBYTE) minutes)  *  60.0; 

return  position; 
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/*********************************************************************** 

PROGRAM : gr idToMi lSec 
AUTHOR: Eric  Bachmann,  Dave  Gay 
DATE: 11  July  1995 

FUNCTION: Convert  a  grid  position  to  a  latitude  and  longitude  in  mil- 
seconds  and  returns  the  result . 
RETURNS :The  latitude  and  longitude  in  milseconds. 
CALLED  BY:navPosit  (nav.cpp) 

CALLS : none 
************************************************************ 

void  fpeGridToMilSec ( int  sig) 

{if  (sig  ==  SIGFPE)  cerr  <<  "floating  point  error  in  gridToMilSec\n" ; } 

latLongMilSec 

navigator : :gridToMilSec (const  grid&  posit) 

{ 

signal (SIGFPE,  fpeGridToMilSec) ; 
latLongMilSec   latLong; 

//converts  grid  in  ft  to  latitude 

latLong . latitude  =  origin . latitude  +  (posit. x  /  LatToFt )  ,- 

//converts  grid  in  ft  to  longitude 

latLong . longitude  =  origin. longitude  + 

HemisphereConversion  *  (posit. y  /  LongToFt); 
return  latLong; 
} 

/*********************************************************************** 

PROGRAM: mi lSecToGr id 

AUTHOR -.Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Convert  a  latitude  and  longitude  expressed  in  milseconds  to 

a  grid  position  based  on  the  lat/long  of  the  grid  origin. 
RETURNS: The  grid  position 
CALLED  BY:navPosit  (nav.cpp) 

initializeNavigator  (nav.cpp) 
CALLS : none 
COMMENTS : altitude  is  always  assumed  to  be  zero. 

*************************************************************************/ 

//Converts  latitude/longitude  to  xy  coords  in  ft  from  origin 

grid 

navigator : :milSecToGrid (const  latLongMilSec&  posit) 

{ 

grid   position; 

position. x  =  (posit . latitude  -  origin . latitude)  *  LatToFt ; 
position. y  =  HemisphereConversion  * 

(posit . longitude  -  origin . longitude)  *  LongToFt; 
position. z  =  0; 


return  position; 


} 
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/*+********************************************************************* 

PROGRAM : getGpsTime 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Parse  the  time  of  a  gps  message. 

RETURNS: The  time  of  the  gps  message  in  seconds 

CALLED  BY:navPosit  (nav.cpp) 

initializeNavigator  (nav.cpp) 

CALLS : none 
****************************************************** 

double 

navigator  : -.getGpsTime  (const  GPSdataSc  rawMessage) 

{ 

UNSIGNED_ONEBYTE      tempchar,  hours,  minutes; 
UNSIGNED_FOURBYTE     tempu4byte; 
double  seconds; 

hours    -    rawMessage [8] ; 
minutes  =  rawMessage [9 ] ; 

tempchar  =  rawMessage [ 10 ] ; 

tempu4byte        =  rawMessage [ 11 ] ; 

tempu4byte        =  ( tempu4byte<<8 )  +  rawMessage [ 12 ] 

tempu4byte        =  ( tempu4byte<<8 )  +  rawMessage [13 ] 

tempu4byte        =  ( tempu4byte<<8 )  +  rawMessage [ 14 ] 

seconds  =  (double) tempchar  +  (( (double) tempu4byte )/ 1 . OE+9 ) ; 


return  hours  *  3  600.0  +  minutes  *  60.0  +  seconds ; 


} 


/*********************************************************************** 

PROGRAM : getGpsVe locity 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Parse  the  velocity  out  of  a  gps  message. 

RETURNS : The  velocitiy  of  the  gps  message  in  feet  per  second 

CALLED  BY:navPosit  (nav.cpp) 

initializeNavigator  (nav.cpp) 

CALLS : none 
************************************************************************* 

double 

navigator : :getGpsVelocity (const  GPSdata&  rawMessage) 

{ 

UNSIGNED_ONEBYTE  tempchar=rawMessage [31] ; 

return  (double) (3 .2804  *  ((tempchar  «  8)  +  rawMessage [32 ] )  /  100.00); 
} 
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F.  GPS.H 

#ifndef  _GPS_H 
#define  _GPS_H 

#include  <iostream.h> 
#include  <fstream.h> 
#include  <conio.h> 

#include  "portbank.h" 
#include  "toetypes.h" 
# include  "gpsbuff.h" 

/*********************************************************************** 

CLASS :gps 

AUTHOR: Eric  Bachmann,  Dave  Gay 
DATE: 11  July  1995 

MODIFIED:  15  May  199  6  by  Eric  Bachmann  &  Randy  Walker 
FUNCTION: Reads  GPS  messages  from  the  GPS  buffer.  Checks  for  valid 
checksum  and  minimun  number  of  satelites  in  view. 

******************************************************** 

class  GPS  { 

public : 

//Class  Constructor 

GPS()  :  portl (COMports. Init (C0M1,  BYTE ( 4 ) ,  b9600, 

NOPARITY,  BYTE(8),  BYTE ( 1 ) ,  XON_XOFF,  messages))  {  } 

//returns  the  latest  gps  position  and  a  flag 
Boolean  gpsPosition  (GPSdata&); 

private : 

//buffer  for  gps  data 
GPSbuffer  messages; 

//instantiates  serial  port  communications  on  comm2 
buf f eredSerialPort&  portl; 

//calculates  the  check  sum  of  the  message 
Boolean  checkSumCheck (const  GPSdata) ; 

}; 
#endif 
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G.  GPS.CPP 

# include  <math.h> 
#include  "gps.h" 

/*********************************************************************** 

NAME : gps Pos i t ion 

AUTHOR:   Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

MODIFIED:  15  May  1995  BY  Eric  Bachmann  &  Randy  Walker 

FUNCTION: Determines  if  an  updated  gps  position  message  is  available  and 
copies  it  into  the  input  argument  ' rawMessage ' .  If  the  message 
has  a  valid  checksum,  was  obtained  with  at  least  three 

satelites  in  view,  and  contained  the  differential  correction,   a  'TRUE' 
is  returned  to  the  caller , indicating  that  the  message  is  valid. 

RETURNS:  TRUE,  if  a  valid  position  message  is  contained  in  the 
input  argument . 

CALLED  BY:  navPosit  (navigator . h) 

CALLS -.Get  (buffer.h) 

checkSumCheck  (gps.h) 
************************************************************** 

Boolean 

GPS : :gpsPosition  (GPSdata&  rawMessage) 

{ 

Boolean  checkSumFlag; 

unsigned  long  Mask(4); 

if  (messages .Get (rawMessage) )  { 

//  Check  for  a  valid  check  sum  and  more  the  3  satelites  and  DGPS 
return  Boolean ( (checkSumCheck (rawMessage ) )  &&  (rawMessage [39]  >  3) 
ScSc    (  (rawMessage  [GPSBLOCKSIZE  -  4  ]  &  Mask)  ==  Mask)); 

} 
else  { 

return  FALSE;   //No  updated  position  is  available. 
} 
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/*********************************************************************** 

PROGRAM : checkSumCheck 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

MODIFIED:  15  May,  1996  by  Eric  Bachmann  &  Randy  Walker 

FUNCTION: Takes  an  exclusive  or  of  bytes  2  through  78  in  a  Motorola  format 

(@@EA)  position  message  and  compares  it  to  the  checksum  of  the 

message . 

RETURNS:  TRUE,  if  the  message  contains  a  valid  checksum 

CALLED  BY:    gpsPosition  (gps) 

CALLS :        none 
********************************************************** 

Boolean 

GPS :: checkSumCheck (const  GPSdata  newMessage) 

{ 

BYTE  chkSum ( 0 ) ; 

for  (int  i  =  2;  i  <  GPSBLOCKSIZE  -  3;  i++)  { 

chkSum  A=  newMessage [i] ; 
} 

return  Boolean (chkSum  ==  newMessage [GPSBLOCKSIZE  -  3 ] ) ; 
} 


H.  INS.H 

#ifndef  _INS_H 
#define  _INS_H 

#  inc lude  <t ime . h> 
# include  <math.h> 
#include  <dos.h> 
#include  <stdio.h> 
#include  <conio.h> 
#include  <fstream.h> 
#include  <iostream.h> 

#include  "toetypes.h" 
#include  "location. h" 
# include  " sampler. h" 

/*********************************************************************** 

CLASS : ins 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Takes  in  linear  accelerations,  angular  rates,  speed  and 

heading  information  and  uses  kalman  filtering  techniques  to  return 

a  dead  reconing  position. 
************************************************************************* 

class  INS  { 
public : 
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//Constructor  initializes  gains 
INS ( ) ; 

~INS()  {} 

//returns  the  ins  estimated  position 
Boolean  insPosition ( stampedSample&) ; 

//Updates  the  x,  y  and  z  of  the  vehicle  posture 
void  correctPosition (const  grid&,  double); 

//Sets  posture  to  the  origin  and  developes  initial  biases 
void  insSetUp (double,  stampedSample* ) ; 


private 


double  posture [6]; 
double  velocities [ 6] 

double  current [3]; 


double  lastGPStime; 

sampler  saml ; 

matrix  rotationMatrix; 


//  ins  estimated  posture  (x  y  z  phi  theta  psi) 

//  ins  estimated  linear  and  angular  velocities 

//  x-dot  y-dot  z-dot  phi-dot  theta-dot  psi-dot 

//  ins  estimated  error  current 

//  (x-dot  y-dot  z-dot) 

//time  of  last  gps  position  fix 

//sampler  instance 

//body  to  euler  transformation  matrix 


double  biasCorrection [8] ; //Software  bias  corrections  for  IMU  rate 

/ /sensors 

//  Kalman  filter  gains. 

float  Konel,  Kone2,  Ktwo,  Kthreel,  Kthree2,  Kfourl,  Kf our2 ; 

//  Finds  the  difference  between  two  times  of  struct  time  type 
double  findDeltaT  (struct  time&  next,  struct  time&  last); 

//  Transforms  from  body  coordinates  to  earth  coordinates 
//  and  removes  the  gravity  component 
void  transf ormAccels  (doublet]); 

//  Transforms  water  speed  reading  to  x  and  y  components 
void  transformWaterSpeed  (double,  doublet]); 

//  Tranforms  body  euler  rates  to  earth  euler  rates, 
void  transf ormBodyRates  (doublet]); 

//  Euler  integrates  the  accelerations  and  updates  the  velocities 
void  updateVelocities  (stampedSample&) ; 

//  Euler  integrates  the  velocities  and  update  the  posture 
void  updatePosture  (stampedSample&) ; 


77 


//  Builds  the  body  to  euler  rate  matrix 
matrix  buildBodyRateMatrix ( ) ; 

//  Builds  the  body  to  earth  rotation  matrix 
void  buildRotationMatrix ( ) ; 

//Calculates  the  imu  bias  correction  during  set  up 
void  calculateBiasCorrections (stampedSample* ) ; 

//Applies  bias  corrections  to  a  sample 
void  applyBiasCorrections (double  sample []); ; 


}; 


//  Post  multiply  a  matrix  times  a  vector  and  return  result, 
vector  operator*  (matrix&,  double [])  ; 

#endif 


I.  INS.CPP 

#include  <iostream.h> 

#include  "signal. h" 

#include  "ins.h" 

#define  SIGFPE  8//  Floating  point  exception 

PROGRAM:  ins  (constructor) 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  19  9  5 

FUNCTION: Constructor  initializes  kalman  filter  gains  and  linear  and 

angular  velocities. 

RETURNS: nothing 

CALLED  BY:    navigator  class 

CALLS :        none 
************************************************************************* 

INS::INS()  :  Konel(O.l),  Kone2(0.5), 
Ktwo(0  .6)  , 

Kthreel (0.5) ,  Kthree2 ( 0 . 5 )  , 
Kfourl(0.5),  Kfour2(0.5) 


{ 


velocities [0]  =  0.0;//  x  dot 

velocities [1]  =  0.0;//  y  dot 

velocities [2]  =  0.0;//  z  dot 

velocities [3 ]  =  0.0;//  phi  dot 

velocities [4]  =  0.0;//  theta  dot 

velocities [5]  =  0.0;//  psi  dot 

//Set  posture  to  straight  and  level  at  the  origin 
posture [0]  -    0.0; 
posture [ 1 ]  =  0.0; 
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posture [2]  =0.0 

posture [3]  =0.0 

posture [4]  =  0.0 

posture [5]  =0.0 


//  Initialize  error  current  to  zero 
current [0]  =  0.0 
current [1]  =  0.0 
current [2]  =  0.0 


} 

PROGRAM:  findDeltaT 
AUTHOR: Eric  Bachmann,  Dave  Gay 
DATE: 11  July  1995 

FUNCTION: Converts  two  times  stored  in  the  time  structure  type  of 
dos.h  into  the  time  in  seconds  and  returns  the  difference. 
RETURNS : difference  in  seconds  between  the  two  input  times. 
CALLED  BY:    insPosit  (ins.cpp) 
CALLS :        none 

Kir***********************************************************************/ 

double 

INS :: findDeltaT  (struct  time&  next,  struct  time&  last) 

{ 

double  present,  past; 

present  =  next.ti_hour  *  3600.0  +  next.ti_min  *  60.0 

+  next.ti_sec  +  next.ti_hund  /  10  0.0; 
past  =  last.ti_hour  *  3600.0  +  last.ti_min  *  60.0 
+  last.ti_sec  +  last.ti_hund  /  100.0; 

//  Did  2400  occur  between  present  and  past? 
if  (present  <  past)  { 
present  +=  86400.0; 
} 

return  present  -  past; 
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/*********************************************************************** 

PROGRAM:  insPosit 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:     Make  dead  reckoning  position  estimation  using  kalman 

filtering.  Inputs  are  linear  accelerations,  angular  rates,  speed  and 

heading.   Primary  input  data  is  obtained  from  a  sampler  object  via  the 

getSample  method.  This  data  is  stored  in  the  sample  filed  of  a 

stampedSample  structure  called  newSample.  The  sample  field  is  then 

used  as  a  working  variable  as  the  linear  accelerations  and  angular 

rates  it  contains  are  converted  to  earth  coordinates  and  integrated 

to  determine  current  velocities  and  posture.  The  data  is  complimentary 

filtered  against  itself,  speed  and  magnetic  heading. 

RETURNS : posit ion  in  grid  coordinates  as  estimated  by  the  INS 

CALLED  BY:    navPosit  (nav.cpp) 

CALLS:        getSample  ( sampler . cpp) 

findDeltaT  (ins. cpp) 

transformBodyRates  (ins. cpp) 

buildRotationMatrix  (ins. cpp) 

transf ormAccels  (ins) 

trans formWaterSpeed  (ins) 
***************************************************** 

void  fpelnsPosit ( int  sig) 
{if  (sig  --   SIGFPE)  cerr  <<  "floating  point  error  in  insPosit\n" ; } 

Boolean 

INS:  :  insPosition  (  stampedSampleSc  newSample) 

{ 

signal  (SIGFPE,  fpelnsPosit); 

double  thetaA,  phiA,  xlncline,  ylncline;  //  Working  variables 
double  waterSpeedCorrection [ 3 ] ;  //  Filter  correction  for  drift 

//  and  water  speed 
if  (saml .getSample (newSample) )  { 

applyBiasCorrections (newSample . sample)  ; 

//Set  output  precision  and  fixed  format 
cout .precision ( 6) ; 
cout . setf ( ios : : fixed) ; 

//Display  linear  accelrations  and  angular  rates 
for  (int  j  =  0;  j  <  8;  j++)  { 

gotoxy ( 59 , j+1 ) ; 

cout  <<  newSample . sample [ j ] ; 
} 

//Display  time  delta  to  the  screen. 

gotoxy (9,25) ; 

cout  «    newSample .del taT; 

xlncline  -   newSample .sample [0 ]  /  GRAVITY; 

ylncline  =  newSample . sample [ 1 ]  /  (GRAVITY  *  cos (posture [4 ])) ; 
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if  (fabs(ylncline)  >  1.0)  { 

static  int  inclineCount ( 0 ) ; 

gotoxy (1,24) ; 

cout  <<  "Inclination  errors:  0"  «    ++inclineCount  <<  endl; 

return  FALSE; 
} 

//Calculate  low  freq  pitch  and  roll 
thetaA  =  asin (xlncline) ; 
phiA  =  -asin (ylncline) ; 

//Transform  body  rates  to  euler  rates, 
trans f ormBodyRates (newSample . sample) ; 

//Calculate  estimated  roll  rate  (phi-dot). 

velocities [3 ]  =  newSample . sample [3 ]  +  Konel  *  (phiA  -  posture [3 ]) ; 
//Calculate  estimated  pitch  rate  (theta-dot). 

velocities [4]  =  newSample . sample [4]  +  Kone2  *  (thetaA  -  posture[4] 
//Calculate  estimated  heading  rate  (psi-dot). 

velocities [5]  =  newSample . sample [5]  +  Ktwo  *  (newSample . sample [7 ] 
posture [5] ) ; 

//integrate  estimated  pitch  rate  to  obtain  pitch  angle 
posture [3]  +=  newSample .deltaT  *  velocities  [  3 ]  ; 
//integrate  estimated  roll  rate  to  obtain  roll  angle 
posture [4]  +=  newSample .deltaT  *  velocities  [4 ]  ; 
//integrate  estimated  yaw  rate  to  obtain  heading 
posture [5]  +-    newSample . deltaT  *  velocities [ 5 ] ; 

//Display  roll  and  pitch 

gotoxy (8, 17) • 

cout  <<  (posture[3]  *  radToDeg); 

gotoxy (8, 18) ; 

cout  <<  (posture [4]  *  radToDeg); 

buildRotationMatrix ( ) ; 

//Transform  accels  to  earth  coordinates 
transf ormAccels (newSample . sample) ; 

//Transform  water  speed  to  earth  coordinates 

transf ormWaterSpeed (newSample . sample [6] ,  waterSpeedCorrection) ; 

//  Subtract  out  previous  velocity  and  apply  statistical  gain 
waterSpeedCorrection [0 ]  =  Kthreel  *  (waterSpeedCorrection [ 0 ]  - 

velocities [ 0 ] ) ; 
waterSpeedCorrection [ 1]  =  Kthree2  *  (waterSpeedCorrection [ 1]  - 

velocities [ 1] ) ; 

//  Determine  filtered  accelerations 

newSample . sample [ 0 ]  +=  waterSpeedCorrection [0] ; 

newSample . sample [ 1]  +=  waterSpeedCorrection [ 1 ] ; 
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//Integrate  accelerations  to  obtain  velocities 
velocities  [  0]  +=  newSample . sample [ 0 ]  *  newSample .deltaT 
velocities [1]  +=  newSample . sample [ 1]  *  newSample .deltaT 
velocities  [2 ]  +=  newSample . sample [2]  *  newSample .deltaT 

//Integrate  velocities  to  obtain  posture 

posture [0]  +=  (velocities [ 0 ]  +  current [ 0 ] )  *  newSample .deltaT; 
posturefl]  +=  (velocities [ 1]  +  current [1])  *  newSample .deltaT; 
posture [2]  +=  velocities [2 ]  *  newSample .deltaT; 

newSample . current  =  sqrt (current [0 ]  *  current [0]  + 

current [1]  *  current [ 1] ) ; 

newSample . sample [ 0]  =  posture [0] 

newSample . sample [ 1]  =  posture [1] 

newSample . sample [2 ]  =  posture [2] 

newSample . sample [3 ]  =  posture [3] 

newSample . sample [ 4 ]  =  posture [4] 

newSample . sample [ 5 ]  =  posture [5] 

//Display  current  location  and  posture 
for  (j  =  0;  j  <  6;  j++)  { 

gotoxy ( 52 , j+12 ) ; 

cout  <<  posture [j]; 

} 

newSample . est . x  =  posture [0] 
newSample . est . y  =  posture [1] 
newSample . est . z  =  posture [2] 

return  TRUE; 

} 
else  { 

return  FALSE;//  New  IMU  information  was  unavailable. 
} 

} 
/*********************************************************************** 

PROGRAM: correct Posit ion 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:  Reinitializes  the  INS  based  on  a  known  position  and  compute 
apparent  current  based  on  past  accumulated  errors  of  the  INS.  It  is 
called  by  the  navigator  each  time  a  new  GPS  (true)  fix  is  obtained. 

RETURNS: void 

CALLED  BY:  navPosit  (nav) 

CALLS : none 
*********************************************************** 

/'  ,  i  '  i 

INS :: correctPosition (const  grid&  truePosit,  double  positTime) 

{ 

double  deltaT; 

//  Correct  for  new  day  if  necessary 
if  (positTime  <  lastGPStime)  { 
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} 


positTime  +=  86400; 
} 

//  Find  time  since  last  gps  fix. 
deltaT  =  positTime  -  lastGPStime; 

//  Detemine  INS  error  since  last  gps  fix 
double  deltaX  =  truePosit.x  -  posture  [0]; 
double  deltaY  =  truePosit.y  -  posture [1]; 

//  Reinitialize  posture  to  known  position  (gps  fix) 

posture [0]  =  truePosit.x; 

posture[l]  =  truePosit.y; 

posture [2]  =  0.0;  //Unit  is  assumed  to  be  on  the  surface 

//  Add  gain  filtered  error  to  previous  errors 
current [0]  +=  Kfourl  *  (deltaX  /  deltaT); 
current [1]  +=  Kfour2  *  (deltaY  /  deltaT); 

//  Display  new  error  current  values 
f or ( int  j  =  0;  j  <  3;  j++)  { 

gotoxy ( 60 , j  +  21 )  ; 

cout  <<  current  [j]; 
} 

//  Display  updated  posture 
for  (j  =  0;  j  <  6;  j++)  { 

gotoxy ( 52 , j  +  12 )  ; 

cout  «    posture  [j]; 

} 

//  Save  the  time  of  the  gps  fix  for  next  calculation 
lastGPStime  -   positTime; 
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/*********************************************************************** 

PROGRAM: insSetUp 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Initializes  the  INS  system.  Sets  the  posture  to  the  origin. 

Initializes  the  heading  using  magnetic  compass  information.  Initilizes 
the  times  of  the  last  GPS  fix  and  last  IMU  information. 
RETURNS: void 

CALLED  BY: initializeNavigator  (nav) 
CALLS : calulateBiasCorrect ions  ( ins ) 

getSample  (sampler) 

buildRotationMatrix  (ins) 

transf ormWaterSpeed  (ins) 

**************************************************** 

void  fpelnsSetUp ( int  sig) 
{if  (sig  ==  SIGFPE)  cerr  <<  "floating  point  error  in  inSetUp\n" ; } 

void 

INS :: insSetUp (double  originTime,  stampedSample*  biasPackets) 

{ 

signal  (SIGFPE,  fpelnsSetUp) ; 

//Initialize  the  sampler 
saml . initSampler ( ) ; 

//set  imu  biases 
calculateBiasCorrections (biasPackets) ; 

//set  initial  true  heading 

posture[5]  =  biasPackets [biasNumber-1 ]. sample [7 ] ; 

//set  initial  speed 

buildRotationMatrix ( ) ; 

transf ormWaterSpeed (biasPackets [biasNumber-1] .sample[6],  velocities) ; 

//  initialize  times 
lastGPStime  =  originTime; 

//Display  initial  error  current  values 
for (int  j  -    0;  j  <  3;  j++)  { 

gotoxy ( 60 , j+21 ) ; 

cout  <<  current [j]; 
} 
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/*********************************************************************** 

PROGRAM : t rans  f ormAcce 1 s 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Trans  forms  linear  accelerations  from  body  coordinates  to 

earth  coordinates  and  removes  the  gravity  component  in  the  z  direction. 

RETURNS: void 

CALLED  BY:    navPosit 

CALLS:        none 
******************************************************* 

void 

INS :: trans formAccels  (double  newSample[]) 

{ 

vector  earthAccels; 

newSample[0]  -=  GRAVITY  *  sin (posture [4 ]) ; 

newSample[l]  +=  GRAVITY  *  sin (posture [ 3 ] )  *  cos (posture [4 ]) ; 

newSample[2]  +=  GRAVITY  *  cos (posture [ 3 ] )  *  cos (posture [ 4 ]) ; 

earthAccels  =  rotationMatrix  *  newSample; 

newSample [0]  =  earthAccels . element [ 0] 
newSample [1]  =  earthAccels .element [1] 
newSample [2]  =  earthAccels . element [2  ] 

} 

/*********************************************************************** 

PROGRAM: trans formWaterSpeed 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Trans forms  water  speed  into  a  vector  in  earth  coordinates 

and  returns  them  in  the  speedCorrection  variable. 

RETURNS: void 

CALLED  BY:    navPosit 

CALLS:        none 
************************************************************************* 

void 

INS : : transf ormWaterSpeed  (double  waterSpeed,  double  speedCorrection [] ) 

{ 

double  water [3]  =  {waterSpeed,  0.0,  0.0}; 

vector  waterVelocities  =  rotationMatrix  *  water; 


speedCorrection  [0]  -   waterVelocities .element [0] 

speedCorrection  [1]  =  waterVelocities . element [ 1] 

speedCorrection  [2]  =  waterVelocities . element [2] 
} 
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/*********************************************************************** 

PROGRAM:  trans f ormBodyRates 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  199  5 

FUNCTION:    Tranforms  body  euler  rates  to  earth  euler  rates 

RETURNS :     none 

CALLED  BY:   insPosit 

CALLS:    buildBodyRateMatrix 

*******************  ****************************************************** 

void 

INS :: trans f ormBodyRates  (double  newSample [] ) 

{ 

vector  earthRates  =  buildBodyRateMatrix ( )  *  & (newSample [ 3 ]) ; 

newSample [3]  =  earthRates . element [ 0 ] 

newSample [4]  =  earthRates . element [ 1 ] 

newSample [5]  =  earthRates . element [2] 
} 

/*********************************************************************** 

PROGRAM:  buildBodyRateMatrix 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:   Builds  body  to  Euler  rate  translation  matrix. 

RETURNS:    rate  translation  matrix 

CALLED  BY:  insPosit 

CALLS :      none 
*************************************************************************/ 

matrix 

INS : : buildBodyRateMatrix ( ) 

{ 

matrix  rateTrans; 

float  tth  -    tan (posture [4] ) , 

sphi  =  sin (posture [3 ]) , 

cphi  -    cos (posture [ 3 ]) , 

cth  =  cos (posture [4] ) ; 


} 


rateTrans. element [0] [0]  =  1.0; 

rateTrans .element [ 0 ][ 1]  =  tth  *  sphi; 

rateTrans . element [ 0 ] [2 ]  =  tth  *  cphi; 

rateTrans .element [1] [0]  =  0.0; 

rateTrans .element [ 1] [ 1]  =  cphi; 

rateTrans .element [ 1 ] [2 ]  =  -sphi; 

rateTrans. element [2] [0]  =  0.0; 

rateTrans .element [2 ][ 1]  =  sphi  /  cth; 

rateTrans .element [2 ] [2]  =  cphi  /  cth; 

return  rateTrans; 
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PROGRAM:  buildRotat ionMatrix 
AUTHOR: Eric  Bachmann,  Dave  Gay 
DATE: 11  July  199  5 

FUNCTION: Sets  the  body  to  earth  coordinate  rotation  matrix. 
RETURNS: void 
CALLED  BY:    insPosit 
insSetUp 

CALLS : none 
it************************************************************************/ 

void 

INS: :buildRotationMatrix( ) 

{ 

float  spsi  =  sin (posture [5] ) , 
cpsi  =  cos (posture [5] ) , 
sth  =  sin (posture [4] ) , 
sphi  =  sin (posture [3 ]) , 
cphi  =  cos (posture [3 ]) , 
cth  =  cos (posture [4] ) ; 

rotat ionMatrix. element [ 0] [ 0 ]  =  cpsi  *  cth; 

rotat ionMatrix. element [ 0 ] [ 1]  =  (cpsi  *  sth  *  sphi)  -  (spsi  *  cphi); 

rotat ionMatrix. element [ 0 ] [2]  =  (cpsi  *  sth  *  cphi)  +  (spsi  *  sphi); 

rotat ionMatrix. element [ 1] [ 0 ]  =  spsi  *  cth; 

rotat ionMatrix. element [ 1] [ 1 ]  =  (cpsi  *  cphi)  +  (spsi  *  sth  *  sphi); 

rotat ionMatrix. element [ 1] [2]  =  (spsi  *  sth  *  cphi)  -  (cpsi  *  sphi); 

rotat ionMatrix. element [2] [ 0 ]  =  -sth; 

rotat ionMatrix. element [2] [ 1]  =  cth  *  sphi; 

rotationMatrix. element [2] [2]  =  cth  *  cphi ; 


/it********************************************************************** 

PROGRAM: post  multiplication  operator  * 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  199  5 

FUNCTION: Post  multiply  a  3  X  3  matrix  times  a  3  X  1  vector  and 

return  the  result . 

RETURNS:      3X1  vector 

CALLED  BY: 

CALLS: None 
it********************************************** 

vector 

operator*  (matrix&  transform,  double  state[]  ) 

{ 

vector  result; 

for  ( int  i  =  0;  i  <  3;  i++)  { 

result .element [ i]  =  0.0; 

for  (int  j  =  0;  j  <  3;  j++)  { 
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result .element [ i]  +=  transform. element [ i ][ j ]  *  state [j]; 
} 
} 
return  result; 

} 

PROGRAM : calculateBiasCorrect ions 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:   Calculates  the  initial  imu  bias  by  averaging  a  number  of 

imu  readings . 

RETURNS :     none 

CALLED  BY:   insSetup 

CALLS :none 
***************************************** 

void  fpeCalculateBiasCorrections ( int  sig) 

{if  (sig  ==  SIGFPE)  cerr  <<  "floating  point  error  in 

CalculateBiasCorrections\n"  ;  } 

void 

INS: : calculateBiasCorrect ions (stampedSample*  biasSample) 

{ 

signal  (SIGFPE,  fpeCalculateBiasCorrections); 

biasCorrection [3 ]  =  0.0 
biasCorrection [4]  =  0.0 
biasCorrection [5]  =  0.0 

for  (int  i  =  0;  i  <  biasNumber;  i++)  { 

while (! saml .getSample (biasSample [ i] ) )  {/*  */}; 
biasCorrection [ 3 ]  +=  biasSample [ i] . sample [ 3 ] 
biasCorrection [4]  +=  biasSample [ i] . sample [4 ] 
biasCorrection [ 5 ]  +=  biasSample [ i] . sample [5] 
} 

//  Find  the  average  of  the  biasNumber  packets 
biasCorrection [3 ]  =  - (biasCorrection [3 ] /biasNumber) 
biasCorrection [4 ]  =  - (biasCorrection [4] /biasNumber ) 
biasCorrection [5]  =  - (biasCorrection [5] /biasNumber ) 

//Output  the  initial  bias  values 
f or ( int  j  =  3;  j  <  6;  j++)  { 

gotoxy (45, j+18) ; 

cout  <<  biasCorrection [ j ] ; 
} 
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/*********************************************************************** 

PROGRAM:  applyBiasCorrect ions 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  199  5 

FUNCTION:    Applies  updated  bias  corrections  to  a  sample. 

RETURNS:     void 

CALLED  BY:   ins Posit 

CALLS :       none 
*********************************************************** 

void 

INS: : applyBiasCorrect ions (double  sample [ ] ) 

{ 

static  const  float  biasWght ( 0 . 9999 ) ,  sampleWght ( 0 . 0001 ) ; 

//Calculate  updated  bias  values 

biasCorrection [3 ]  =  (biasWght  *  biasCorrection [ 3 ] ) 

-  (sampleWght  *  sample [3]), • 
biasCorrection [4 ]  =  (biasWght  *  biasCorrection [4] ) 

-  (sampleWght  *  sample[4]); 
biasCorrection [5]  =  (biasWght  *  biasCorrection [ 5] ) 

-  (sampleWght  *  sample [5]); 

//Apply  the  bias  to  the  sample 
sample [3]  +=  biasCorrection [ 3 ] 
sample [4]  +=  biasCorrection [4] 
sample[5]  +~   biasCorrection [ 5] 

//  Output  the  biases 

f or ( int  j  =  3 ;  j  <  6 ;  j++)  { 

gotoxy (45, j+18) ; 

cout  <<  biasCorrection [j ] ; 
} 
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J.  SAMPLER  .H 

#ifndef  SAMPLER_H 

#define  SAMPLER_H 

#include  <time.h> 
#include  <math.h> 
#include  <dos.h> 
#include  <conio.h> 
#include  <stdio.h> 


# include  "toetypes.h" 
#include  " location. h" 
ttinclude  "a2d.h" 
# include  "compass.h" 

#define  MAX_SAMPLE_NUM  10  0  0 
const  int  INBUFFSIZE  =  512; 

/*********************************************************************** 

CLASS : sampler 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 15  May  1996 

FUNCTION: Formats,  timestamps,  low  pass  filters  and  limit  checks  IMU, 

water-speed  and  heading  information. 
COMMENTS:  This  class  is  extremely  dependent  upon  the  specific 

hardware  configuration.  It  is  designed  to  isolate  to  the  INS  from 

these  particulars. 
******************************************************* 

class  sampler  { 

public : 

//Class  Constructor 

sampler ()  :  samplelndex ( 0 )  ,  subSamplelndex ( 0 ) , 

samplePeriod(a2dl.chcnt  *  a2dl.delta_t  *  0.000001)0 

//Initializes  Sampler 
Boolean  initSampler ( ) ; 

//checks  for  the  arrival  of  a  new  sample  and  formats  it. 
Boolean  getSample ( stampedSample& ) ; 

private : 

compass  compl ;    //Compass  instance 

a2dClass  a2dl;     //A2D  instance 

float  sample [MAX_SAMPLE_NUM] [8] ; 

int  subSamplelndex; 
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int  samp le Index; 

int  sampleCount; 

float  samplePeriod; 

Boolean  readSamples (stampedSample&  newSample); 

void  f ilterSample (stampedSample&  newSample) ; 

void  formatSample  (  stampedSampleSc  newSample); 

void  increment ( int&  index) 

{  if  (++index  ==  MAX_SAMPLE_NUM )  index  =  0;} 

void  decrement ( int&  index) 

{  if  (--index  <  0)  index  =  MAX_SAMPLE_NUM  -  1;} 


}; 
#endif 
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K.  SAMPLER.CPP 

#include  <iostream.h> 
#include  <conio.h> 
#include  " sampler. h" 

/*********************************************************************** 

PROGRAM: in it Sampler 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 12  May  1995 

MODIFIED:  15  May  1996  by  Eric  Bachmann  &  Randy  Walker 

FUNCTION: Initializes  the  compass  and  the  A2D  module. 

RETURNS : TRUE 

CALLED  BY: 

CALLS:  initCompass ( ) ,    A2D  member    functions 

**************************************************************** 

Boolean 

sampler : : initSampler ( ) 

{ 

compl . initCompass ( ) ; 

a2dl . setRmsOf f ( ) ; 
a2dl . setSequencer ( ) ; 
a2dl . lockTrigger ( ) ; 
a2dl.resetFifo( ) ; 
a2dl.setFifo() ; 
a2dl .unlockTrigger ( ) ; 
a2dl . setTrigger ( ) ; 

return  TRUE; 
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PROGRAM:  getSample 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 15  May  199  6 

FUNCTION: Prepares  raw  sample  data  for  use  by  the  INS   object 

RETURNS : TRUE ,  if  a  valid  sample  was  obtained 

CALLED  BY:insPosit  (ins) 

insSetup  (ins) 
CALLS:     Get  (packetBuf f er ) 

createSample  (sampler) 

formatSample  (sampler) 

inLimitSample  (sampler) 
****  +  +  +  ************  +  *****  +  +  +  **  +  +  +  +  *  +  +  *  +  +  +  ******************■*■**■**•****•**•*  +  +/ 

//checks  for  the  arrival  of  a  new  sample 

Boolean 

sampler: : getSample ( stampedSample&  newSample) 

{ 

if  ( readSamples (newSample) )  { 

filterSample (newSample) ; 

formatSample (newSample) ; 

return  TRUE; 
} 
return  FALSE;   //Sample  packet  not  available 
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/*********************************************************************** 

PROGRAM:  readeSamples 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 12  May  19  9  6 

FUNCTION:  Retrieves  all  samples  of  the  IMU,  water  speed,  and  depth 

that  are  present  in  the  A2D  FIFO  until  the  FIFO  is  EMPTY.   Calculates 

delta_t . 

RETURNS:   Boolen :  TRUE  -  There  were  new  samples  pulled  from  the  FIFO 

FALSE  -  There  were  no  new  samples 

CALLED  BY:  getSample 

CALLS:      getFifoStatus ( ) ,  getFif oData ( ) 
********************************************************* 

Boolean 

sampler:  :  readSamples  (  stampedSampleSc    newSample) 

{ 

//Did  the  FIFO  overflow? 

if  (a2dl. getFifoStatus ( )  ==  FULL)  { 

gotoxy ( 17 , 20 ) ; 

cout  <<  "FIFO  Overflowed,  execution  terminated..."  <<  endl ; 

exit (1) ; 
} 

//Does  the  FIFO  have  new  samples? 
if  (a2dl. getFifoStatus ( )  !=  EMPTY)  { 

sampleCount  =  0;  //Counts  the  number  of  samples  taken 

//Empty  the  FIFO 

while  (a2dl .getFifoStatus ( )  !=  EMPTY)  { 


sample [samplelndex] [subSampleIndex++]  =  a2dl .getFif oData () ; 

//Has  it  pulled  one  sample  of  each  channel  from  the  FIFO? 
if  (subSamplelndex  ==8)  { 

subSampleIndex=  0 ; 

increment (samplelndex) ;  //set  to  record  next  sample 

++sampleCount ; 
} 

} 

if  (sampleCount  >  0)  { 

//calculate  time  delta 

newSample .deltaT  =  sampleCount  *  samplePeriod; 

gotoxy (1,  19)  ; 
print f  (  "       "  )  ; 
gotoxy (1,19); 

print f ( "sampleCount :  %d" , sampleCount ) ; 
return  TRUE; 
} 
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else  { 

return  FALSE; 
} 


else  { 


//No  new  samples 
return  FALSE; 


PROGRAM:  createSample 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 15  May  199  6 

FUNCTION:   Low  pass  filters  by  averaging  over  all  samples  received  since 

the  last  sample.. 

RETURNS:    void 

CALLED  BY:  getSample 

CALLS :      none 
************************************* 

void 

sampler: : f ilterSample (stampedSample&  newSample) 

{ 

for  (int  i  =  0;  i  <  8;  i++)  { 

newSample . sample [ i ]  =  0; 
} 

int  j ( samplelndex) ; 

for  (i  =  0;  i  <  sampleCount;  i++)  { 


decrement ( 
newSample . 
newSample . 
newSample . 
newSample . 
newSample . 
newSample . 
newSample . 
newSample . 


j)  ; 

sample 

sample 

sample 

sample 

sample 

sample 

sample 

sample 


+=  sample[j][0]  /  sampleCount 

+=  sample [j ][ 1 ]  /  sampleCount 

+=  sample [j ] [2]  /  sampleCount 

+=  samplefj] [3]  /  sampleCount 

+=  sample[j][4]  /  sampleCount 

+=  sample[j][5]  /  sampleCount 

+=  sample [j ][ 6]  /  sampleCount 

+=  samplefj] [7]  /  sampleCount 
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/*********************************************************************** 

PROGRAM: format Sample 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 15  May  1996 

FUNCTION: Converts  integers  representing  voltage  readings  into 

units  which  are  useable  by  the  INS. 

RETURNS:  void 

CALLED  BY:getSample 

CALLS : none 
************************************************* 

//Calls  the  methods  to  convert  the  voltages  to  real  world  units 

void 

sampler  ::  formatSample  (  stampedSampleSc  newSample) 

{ 

newSample . sample [ 0 ]  =  xAccelUnits (newSample . sample [ 0 ] ) 

newSample . sample [ 1]  =  yAccelUnits (newSample . sample [ 1] ) 

newSample . sample [2 ]  =  zAccelUnits (newSample . sample [2 ] ) 

newSample . sample [3 ]  =  pUnits (newSample . sample [3 ] ) 
newSample . sample [4 ]  =  qUnits (newSample . sample [4] ) 
newSample . sample [ 5]  =  rUnits (newSample . sample [ 5 ] ) 

newSample . sample [ 6 ]  =  waterSpeedUnits (newSample . sample [ 6 ]) ; 
newSample . sample [7 ]  =  compl .getHeading ( ) ; 


/ 
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L.  COMPASS.H 

#ifndef  _COMPASS_H 
#define  _COMPASS_H 

#include  <iostream.h> 
#include  <f stream. h> 
#include  <conio.h> 

#include  "portbank.h" 
#include  "toetypes.h" 
#include  "location.h" 
#include  "compBuff.h" 

/*********************************************************************** 

CLASS : compass 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

MODIFIED:  15  May,  1996  by  Eric  Bachmann  &  Randy  Walker 

FUNCTION: Reads  compass  messages  from  the  compass  buffer.  Checks  for 

validchecksum.  Corrects  heading  for  megnetic  variation.  Heading  is 

continous.  There  is  no  branch  cut  at  360  degrees. 
*************************************************************** 

class  compass  { 

public : 

//Class  Constructor 

compass ()  :  port2 (COMports . Init (COM2 ,  BYTE ( 3 ) ,  b9600, 

NO PARITY,  BYTE (8),  BYTE ( 1 ) ,  NONE,  headings)) 

{} 

//Initialize  currentHeading 
float  initCompass ( ) ; 

//returns  the  latest  heading 
float  getHeading ( ) ; 

private : 

//buffer  for  compass  data 
compBuffer  headings ; 

//Maintains  the  most  recently  obtained  heading, 
float  currentHeading; 

//instantiates  serial  port  communications  on  comm2 
buf f eredSerialPort&  port2; 

//calculates  the  check  sum  of  the  message 
Boolean  checkSumCheck (const  compData&) ; 

//Parses  the  heading  out  of  a  compass  message. 
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float  parseCompData (const  compData&,  const  BYTE); 

//  Convert  magnetic  direction  based  on  magnetic  variation, 
float  trueHeading (const  float); 

//  Returns  the  heading  without  branch  cuts 
float  continousHeading (const  float) ; 

}; 
#endif 

M.  COMPASS.CPP 

#include  <math.h> 
# include  " compass. h" 

float 

compass : : initCompass ( ) 

{ 

float  tempHeading; 
compData  rawMessage; 

while  ( (headings .Get (rawMessage) ==FALSE) 

II  ( checkSumCheck( rawMessage ) ==FALSE) )  {} 

tempHeading  =  parseCompData (rawMessage,  'C')  *  degToRad; 
currentHeading  =  continousHeading (trueHeading (tempHeading) ) ; 

return  currentHeading; 
} 
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/*********************************************************************** 

NAME : getHeading 

AUTHOR:   Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: 

Determines  if  an  updated  gps  position  message  is  available  and 

copies  it  into  the  input  argument  'rawMessage' .  If  the  message 

has  a  valid  checksum  and  was  obtained  with  atleast  three 

satelites  in  view,   a  'TRUE'  is  returned  to  the  caller, 

indicating  that  the  message  is  valid. 

RETURNS:      TRUE,  if  a  valid  position  message  is  contained  in  the 

input  argument . 

CALLED  BY:    navPosit  (navigator .h) 

CALLS:Get  (buffer. h) 

checkSumCheck  (gps.h) 
*************************************************************** 

float 

compass : : getHeading ( ) 

{ 

float  tempHeading; 

Boolean  checkSumFlag; 

compData  rawMessage; 

if  ( (headings .Get (rawMessage) )  &&  (checkSumCheck (rawMessage) ) )  { 

tempHeading  =  parseCompData (rawMessage,  'C')  *  degToRad; 
currentHeading  = 

continousHeading (trueHeading (tempHeading) ) ; 

return  currentHeading; 
} 
else  { 

return  currentHeading;  //  No  updated  position  is  available. 
} 

} 

BYTE 

asciiToHex(BYTE  letter) 

{ 

if  (letter  >=  ' A' )  { 

return  (letter  -  'A'  +  10); 
} 
else  { 

return  (letter  -  48); 
} 
} 
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/*********************************************************************** 

PROGRAM : checkSumCheck 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: 

Takes  an  exclusive  or  of  bytes  2  through  78  in  a  Motorola  format  (@@EA) 

position  message  and  compares  it  to  the  checksum  of  the  message  of  the 

message . 

RETURNS:  TRUE,  if  the  message  contains  a  valid  checksum 

CALLED  BY:    gpsPosition  (gps) 

CALLS :        none 
************************************************************* 

Boolean 

compass :: checkSumCheck (const  compData&  newMessage) 

{ 

BYTE  calChkSum ( 0 ) ; 
BYTE  mesChkSum(O) ; 

for  ( int  i  =  1;  newMessage [ i ]  !=  ' *';  i++)  { 

calChkSum  A=  newMessage [ i] ; 
} 

mesChkSum  =  asciiToHex (newMessage [ i+1 ] )  *  16 
+  asciiToHex (newMessage [ i+2] ) ; 


return  Boolean (calChkSum  ==  mesChkSum); 
} 


KM) 


/*********************************************************************** 

PROGRAM:    trueHeading 

AUTHOR:     Eric  Bachmann,  Dave  Gay 

DATE:       11  July  1995 

FUNCTION:   Convert  magnetic  direction  to  true  based  on  local  magnetic 

variation . 

RETURNS:    true  heading 

CALLED  BY:   insPosit 

insSetUp 

CALLS :       none 
********************************************************************* 

float 

compass :: trueHeading (const  float  magHeading) 

{ 

static  double  twoPi(2.0  *  M_PI); 

double  trueHeading  -   magHeading  +  RADIANMAGVAR; 

if  (trueHeading  >  twoPi)  { 

trueHeading  -=  twoPi; 
} 

return  trueHeading; 
} 
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PROGRAM:  cont inousHeading 

AUTHOR:  Eric  Bachmann 

DATE:  11  July  1995 

FUNCTION:  Maintains  track  of  branch  cuts  and  returns  a  continous  heading 

RETURNS:  continous  true  heading 

CALLED  BY:  ins Posit 

insSetUp 

CALLS:       none 
♦  ••••it********************************* 

float 

compass :: continousHeading (const  float  trueHeading) 

{ 

const  float  twoPi(2.0  *  M_PI); 

static  int  branchCutCount ( 0 ) ; 

static  float  previousHeading (trueHeading) ; 

if  ((4.71  <  previousHeading)  &&  (trueHeading  <  1 . 57 ) ) { 

++branchCutCount ;  //Went  through  North  in  a  right  hand  turn 

} 
else  { 

if  ((1.57  >  previousHeading)  &&  (trueHeading  >  4.71))  { 

--branchCutCount ; //Went  through  North  in  a  left  hand  turn 

) 
} 

previousHeading  =  trueHeading; 

return  trueHeading  +  (branchCutCount  *  twoPi); 
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/*********************************************************************** 

PROGRAM:  parseHeading 

AUTHOR:  Eric  Bachmann 

DATE:  11  July  1995 

FUNCTION:  Parses  the  heading  out  of  a  compass  message. 

RETURNS:  the  message  heading  as  a  float 

CALLED  BY:   insPosit 

insSetUp 

CALLS :       none 
*************************************************************** 

float 

compass :  :parseCompData  (const  compDataSc  rawMessage,  const  BYTE  key) 

{ 

float  dataSum(O); 

f or ( int  j  =  0 ;  rawMessage [ j ]  !=  key;  j++){} 

f or ( int  i  =  0 ;  rawMessage [i  +  j]  !=  '.';  i++){) 
switch  (i)  { 
case  3  : 

dataSum  =  (rawMessage [j ]  -  48)  *  100.0  + 

(rawMessage [j+1]  -  48)  *  10.0  + 

(rawMessage [j +2]  -  48)  + 

( rawMessage [j +4]  -  48)  *  0.1; 
break; 

case  2  : 

dataSum  =  (rawMessage [j ]  -  48)  *  10.0  + 

(rawMessage [j+1]  -  48)  + 

(rawMessage [j +3]  -  48)  *  0.1; 
break; 


} 


case  1 


dataSum  -    ( rawMessage [j ]  -  48)  + 

( rawMessage [j +2]  -  48)  *  0.1; 


break; 

} 

return  dataSum ; 
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N.  A2D.H 

[ 

#ifndef 

_A2D_H 

#define 

_A2D_H 

#include 

<dos .h> 

#include 

<math.h> 

#include 

<conio .h> 

#include 

<stdio.h> 

#include 

<stdlib.h> 

#include 

<stdarg .h> 

#include 

<iostream.h> 

#include 

<f stream. h> 

//ESP  A2D  General  Global  Definitions 

#define  DEFBASE   0x100   //Base  address  SEL=l->0x300  &  SEL=O->0xlO0 

#define  FIFOSIZE   1000   //FIFO  size  (MAX=1000  decimal) 

#define   MAXCHAN   0x10   //Max  channels 


//ESP  A2D  Status  Register  Definitions 

//BASE+02h:  011D  DDDD 

#define  INT_STAT  0x10    //  0001  0000  INTERRUPT  STATUS 

#define  TRG_STAT  0x08    //  0000  1000  TRIGGER  STATUS 


(1=IRQ  Pending) 
( l=Triggered) 


#define  FULL  0x01 
#define  HALF  0x05 
#define  EMPTY     0x0  6 


//  0000  0001  FIFO  FULL 

//  0000  0101  FIFO  HALF  FULL 

//  0000  0110  FIFO  EMPTY 


(001=Full) 
(101=Half  Full) 
(110=Empty) 


//ESP  A2D  Control  Register  Definitions 

//BASE+08h:  DDDD  DDDD 

//BASE+0  9h:  DDDD  DDRR 

#define  GATE10UT  0x0008  //  0000  0000  0000  1000  GATE10UT 


(Always  Driven) 


ttdefine  TRG_POS 
#define  SET_TRG 
#define  RST_TRG 
#define  INT  EN 


0x0010  //  0000  0000  0001  0000  TRIG  POS 

0x0020  //  0000  0000  0010  0000  TRIG  SET 

0x0040  //  0000  0000  0100  0000  TRIG  CLR 

0x0080  //  0000  0000  1000  0000  IRQ  ENAB 


(Trig  on  + I -) 
(Active  LOW) 
(Active  LOW) 
(Active  HIGH) 


#define  DIFF 
#define  RMS 


0x0400   //  0000  0100  0000  0000  DIFF/SE 
0x0800   //  0000  1000  0000  0000  RMS  Mode 


(1=DIFF  0=SE) 
(l=ON    0=OFF) 


#define  CAL 
#define  PRG_SEQ 
#define  ACDC 
#define  SAM_SEQ 
ttdefine  RST  FIFO 


0x1000  //  0001  0000  0000  0000  CAL   Mode  (l=ON  0=OFF) 

0x1000  //  0001  0000  0000  0000  SEQ   Mode  (1=PRG  0=RUN) 

0x2000  //  0010  0000  0000  0000  ACDC  Mode  (1=DC  0=AC) 

0x4000  //  0100  0000  0000  0000  SAMP/SEQ   (1=SEQ  0=SAMP; 

0x8000  //  1000  0000  0000  0000  FIFO  Reset(l=EN  0=REW) 


//ESP  A2D  Useful  Definitions 

#define  CLRRATE   0xFFF8  //  CLEAR  RATE  TO  HIGHEST  RATE 


104 


//  CLASS  NAME:  a2dClass 

//  AUTHOR:  Randy  Walker 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Provides  for  the  software  operation  of  the  A2D  module, 
//it************************************** 

//Class  Definition  for  the  A2D  Class 
class  a2dClass  { 

public : 

//Class  Constructor;  reads  a2d.cfg  file,  initializes  hardware 
a2dClass ( ) ; 

//Reads  a2d.cfg  configuration  file 
void  readConf igFile ( ) ; 

//Sets  address  mapping 
void  initSysAddr (void) ; 

//Initializes  the  A2D  Control  Register 
void  initHardware (void) ; 

//Print  out  the  variable  ctrlw,  for  debug  purposes 
void  printCtrlw ( void) ; 

//Sets  the  A2D  Control  Register  for  Single-Ended  mode 
void  setSe (void) ; 

//Sets  the  A2D  Control  Register  for  Differential  mode 
void  setDif f ( void) ; 

//Loads  sequencer  memory  with  channel  data 

void  setChannel (unsigned  seq, unsigned  ch, unsigned  glO, unsigned  g2 ) 

//Sets  sequencer  to  program  mode 
void  setProgSeq (void) ; 

//Sets  sequencer  to  run  mode 
void  setRunSeq (void) ; 

//Loads  sequencer  address  counter  with  number  of  channels  to  scan, 
void  setCount (unsigned  nch) ; 

//Sets  AC  or  DC  Coupling 
void  setAcDc (unsigned  acdc); 

//Prevents  triggering 
void  lockTrigger (void) ; 

//Allow  the  triger  to  function 
void  unlockTrigger ( void) ; 
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//Toggle  the  trigger  (software  triggering] 
void  setTrigger (void) ; 

//Clears  the  trigger 
void  resetTrigger (void) ; 


//Switches  in  the  RMS  measurement  chip 
void  setRmsOn (void) ; 

//Switches  out  RMS  measurement  chip 
void  setRmsOf f (void) ; 

//Sets  the  A2D  module  to  sequencer  mode 
void  setSequencer (void) ; 

//Sets  the  A2D  module  to  sampler  mode 
void  setSamplerRate (unsigned) ; 

//Set  GATE10UT  bit  of  control  word  high 
void  gateloutOn (void) ; 

//Set  GATE10UT  bit  of  control  word  low 
void  gateloutOf f (void) ; 

//Sets  timer  channel  1  to  square-wave  input 
void  squareWaveTimerl (unsigned) ; 

//Initialize  the  A2D  timing  using  timer  2 
void  initTiming (unsigned  dt); 

//Rewind  FIFO  to  beginning  of  memory 
void  resetFifo (void) ; 

//Enable  FIFO  to  acquire  data 
void  setFifo (void) ; 

//Returns  th  state  of  the  fifo 
unsigned  getFifoStatus (void) ; 

//Returns  next  data  word  stored  in  FIFO 
signed  getFifoData (void) ; 

//Program  timer  channel  0  to  set  the  desired  interrupt  rate 
void  setlntRate (unsigned  intrate) ; 

//Locksout  the  interupt  request  line 
void  intOf f (void) ; 

//Enables  system  interuppt  request 
void  intOn (void) ; 

//Sets  the  trigger  level;  trigger  level  (0=-10V,  128=0V,  255=+10V) 
void  setTriggerLevel (unsigned  tl); 
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//Sets  falling  or  rising  edge  trigger 
void  setTriggerPosition (unsigned  tp) ; 

//Calibrates  zero  offset  error 
void  zeroOf f set (void) ; 

//Grounds  the  two  differential  inputs  for  zero  adjust 
void  grndlnput ( void) ; 

//Ungrounds  the  two  differential  inputs 
void  f reelnput ( void) ; 

//Adjust  the  trimmer  on  the  PGA 
void  zeroAdjust (void) ; 


int  chcnt ; 
unsigned  delta_t; 


//Number  of  channels  to  sequence 
//period  between  channels 


private 


unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 


ctrlw; 
seqcnt ; 
mode_sel ; 
mode_acdc ; 
samprate; 
samp index; 
seqaddr [MAXCHAN] 
chan[MAXCHAN] ; 
glO [MAXCHAN] ; 
g2 [MAXCHAN] ; 


//Holds  A2D  Control  Register  update  values 

//Sequence  Counter 

//Single-ended  or  Differential 

//AC/DC  Coupling 

//Sample  Rate  in  Recurrent  Mode 

//Which  Channel  to  Sample  in  Recurrent  Mode 

//Sequencer  Address 

//Channel 

//xlO    Gain 

//x2    Gain 


}; 


#endif 


0.  A2D.CPP 


# include    "a2d.h" 


//ESP   A2D  Addresses 


unsigned 

BASE 

=  DEFBASE; 

BASE  I/O   ADDR 

[BASE] 

0 

unsigned 

FIFO 

=  0x00 

// 

FIFO  READ  ADDR 

[00- 

01] 

(R) 

unsigned 

MEM 

=  0x00 

// 

SEQUENCER  ADDR 

[00- 

01] 

(W) 

unsigned 

STAT 

=  0x02 

// 

STATUS  REGISTER 

[02] 

(R) 

unsigned 

COUNT 

=  0x02 

// 

SEQUENCER  ADDR  PTR 

[02] 

(W) 

unsigned 

TIMERO 

=  0x04 

// 

TIMER  0 

[04] 

(R/W) 

unsigned 

TIMER1 

=  0x05 

// 

TIMER  1 

[05] 

(R/W) 

unsigned 

TIMER2 

=  0x06 

// 

TIMER  2 

[06] 

(R/W) 

unsigned 

TIMERC 

=  0x07 

// 

TIMER  CONTROL  WORD 

[07] 

(R/W) 

unsigned 

CNTL 

=  0x08 

// 

A2D  CONTROL  REGISTER 

[08- 

09] 

(W) 

unsigned 

DAC 

=  OxOC 

// 

DAC  DATA 

[0C] 

(W) 
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//********************************************************************* 

//    FUNCTION  NAME:  a2dClass ( ) 

//  AUTHOR:  Randy  Walker 

//  DATE:  27  March  19  9  6 

//  DESCRIPTION:  Sets  defaults,  reads  a2d.cfg  file,  initializes  address  map 

/ /  and  hardware 

//  RETURNS:  void 

//  CALLS:  readConf igFile ( ) ,  initSysAddr ( ) ,  initHardware ( ) 

//  CALLED  BY:  Object  declaration 
//********************************************************************* 

a2dClass : :a2dClass (void) 

{ 

ctrlw=0; 
seqcnt=l ; 
mode_sel=0 ; 
mode_acdc=l ; 
delta_t=3; 
chcnt=l; 
samprate=0 ; 
sampindex=0 ; 
readConf igFile ( ) ; 
initSysAddr ( ) ; 
initHardware ( ) ; 

} 

//********************************************************************* 

//  FUNCTION  NAME:  readConf igFile ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Reads  the  a2d.cfg  file  and  sets  variables 

//  RETURNS:  void 

/ /  CALLS :  none 

//  CALLED  BY:  a2d  class  constructor 
//************************************************************  *  *  ******* 

void  a2dClass :: readConf igFile ( ) 

{ 

FILE  *conf igFile; 
char  junk [128] ; 

if  ((conf igFile  =  f open  (  "a2d . cf g" ,  "r"))  ==  NULL)  { 

fprintf (stderr,  "Cannot  open  file  A2D.CFG. . . \n" ) ; 

exit (1) ; 
) 

f scanf (conf igFile ,  " %x%s " , &seqcnt , junk)  ; 

if  (seqcnt  =  =  0  I  I seqcnt>OxOF)  {   //seqcnt  must  be  1-F  (15  max  in  seq  mode) 

cout  <<  "\nseqcnt  out  of  range  in  A2D.CFG. . . \n" ; 

exit (1) ; 
} 

f scanf (conf igFile, "%d%s" , &mode_sel , junk) ; 
if  (mode_sel  !=0  &&  mode_sel  ! =  1){ 

cout  <<  "\nmode_sel  out  of  range  in  A2D.CFG. . . \n" ; 
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exit (1)  ; 
} 

f scanf (conf igFile, "%d%s " , &mode_acdc, junk) ; 
if  (mode_acdc  ! =0  &&  mode_acdc  !=  1){ 

cout  «    " \nmode_acdc  out  of  range  in  A2D.CFG. . . \n" ; 

exit  ( 1 )  ; 
} 

f scanf (conf igFile, "%x%s" , & chcnt , junk) ; 

if  (chcnt  ==011  chcnt  >  OxOF) {  //chcnt  must  be  1-F  (15  max  in  seq  mode; 

cout  <<  " \nchcnt  out  of  range  in  A2D.CFG. . . \n" ; 

exit  (1)  ; 
} 

fscanf (conf igFile,  "%d%s"  ,  &delta_t, junk)  ; 
if  (delta_t  <  3  II  delta_t  >  8192) ( 

cout  <<  "\ndelta_t  out  of  range  in  A2D . CFG . . . \n" ; 

exit (1)  ; 
} 

if  (delta_t  <  6  &&  chcnt  >  1){ 

cout  <<  "\ndelta_t  must  be  >  6  for  chcnt  >  l...\n"; 

exit ( 1 )  ; 
} 

fscanf (conf igFile,  "%d%s"  ,  &samprate, junk)  ; 
if  (samprate  >  7)  { 

cout  <<  " \nsamprate  out  of  range  in  A2D.CFG . . . \n" ; 

exit (1)  ; 
} 

fscanf (conf igFile,  " %x%s" ,  &sampindex, junk)  ; 
if  (sampindex  >  OxOF) { 

cout  <<  " \nsampindex  out  of  range  in  A2D.CFG. . . \n" ; 

exit (1)  ; 
} 

for  (int  i=0 ; i<seqcnt ; i++ ) 

fscanf (conf igFile, "%x%x%x%x" ,&seqaddr [i] , &chan[i] , &gl0 [i]  ,  &g2 [i] )  ; 
f close (conf igFile) ; 
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// 
// 
// 
// 
// 
// 
// 
// 
// 

vo 

{ 


********************************************************************* 

FUNCTION  NAME:  initSysAddr ( ) 

AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

DATE:  27  March  1996 

DESCRIPTION:  Sets  system  address  mappings 

RETURNS:  void 

CALLS :  none 

CALLED  BY:  a2d  class  constructor 
********************************************************************* 

id  a2dClass : : initSysAddr (void) 


//clear 

FIFO 

MEM 

STAT 

COUNT 

TIMERO 

TIMER 1 

TIMER2 

TIMERC 

CNTL 

DAC 


BASE 


Sc  = 

&= 
5c= 
&= 
&= 
&= 
&  = 


OxOF 
OxOF 
OxOF 
OxOF 
OxOF 
OxOF 
OxOF 


&  =  OxOF 


&  = 


//set  BASE 


FIFO 

MEM 

STAT 

COUNT 

TIMERO 

TIMER 1 

TIMER2 

TIMERC 

CNTL 

DAC 


OxOF 
OxOF 


=  BASE 
=  BASE 
=  BASE 
=  BASE 
=  BASE 
=  BASE 
=  BASE 
=  BASE 
=  BASE 
=  BASE 


// 

FIFO  READ  ADDRESS 

too, 

31] 

(R) 

// 

SEQENCER  MEM  ADDRESS 

[00, 

31] 

(W) 

// 

STATUS  REGISTER 

[02] 

(R) 

// 

SEQENCER  ADDRESS  PTR 

[02] 

(W) 

// 

TIMER  0 

[04] 

(R/W) 

// 

TIMER  1 

[05] 

(R/W) 

// 

TIMER  2 

[06] 

(R/W) 

// 

TIMER  CONTROL  WORD 

[07] 

(R/W) 

// 

CONTROL  REGISTER 

[08] 

(R/W) 

// 

DAC  DATA 

[0C] 

(W) 

// 

FIFO  READ  ADDRESS 

[00, 

01] 

(R) 

// 

SEQENCER  MEM  ADDRESS 

[00, 

01] 

(W) 

// 

STATUS  REGISTER 

[02] 

(R) 

// 

SEQENCER  ADDRESS  PTR 

[02] 

(W) 

// 

TIMER  0 

[04] 

(R/W) 

// 

TIMER  1 

[05] 

(R/W) 

// 

TIMER  2 

[06] 

(R/W) 

// 

TIMER  CONTROL  WORD 

[07] 

(R/W) 

// 

CONTROL  REGISTER 

[08] 

(R/W) 

// 

DAC  DATA 

[0C] 

(W) 

110 


//  FUNCTION  NAME:  initHardware ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  the  A2D  Control  Register  to  0020  and  sets  the 

//  data  member,  ctrlw=0060;  initializes  the  module  setup 

//  for  software  triggering  of  the  A2D.   Programs  each 

/ /  channel . 

//  RETURNS:  void 

//  CALLS:  outpw() 

//  CALLED  BY:  a2d  class  constructor 
//******************************************************************+** 

void  a2dClass: : initHardware (void) 
{ 

outpw(CNTL, SET_TRG) ; 

ctrlw  =  SET_TRG | RST_TRG ; 

if (mode_sel  ==  0) 
setSe( ) ; 

else 

setDiff ( ) ; 

f or ( int  i  =  0 ; i  <  chcnt;i++){ 

setChannel ( seqaddr [ i ] , chan [ i ] , glO [ i ] , g2 [ i ] ) • 
} 

setAcDc (mode_acdc) ; 
initTiming (delta_t ) ; 
setCount (chcnt ) ; 


//it************************************** 

//  FUNCTION  NAME:  printCtrlw() 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Print  A2D  control  register  var,  ctrlw. 

//  The  variable  is  used  to  set  a  byte  in  the 

//  ESP  A2D  control  register  at  BASE  +  08h/09h 

//  Used  during  application  code  debug 

//  RETURNS:  void 

/ /  CALLS :  none 

//  CALLED  BY:  none 

void  a2dClass : :printCtrlw(void) 
{ 

printf ( "ctrlw:  %04x\t",  ctrlw) ; 
for  (int  i=0x00;i<0xl0;i++) { 

printf  (  "%i"  ,(  (ctrlw»0x0F-i )  &  1)  )  ; 
if  ((i+l)%4==0) 
printf ( "  " ) ; 
} 

} 
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//it******************************************************************** 

//  FUNCTION  NAME:  setSe ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Sets  ctrlw  for  single  ended  mode  and  writes  ctrlw  to 

//  A2D  Control  Register 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  initHardware ( ) 
//******************************** 

void  a2dClass : : setSe ( void) 

{ 

ctrlw  &=  -DIFF; 
outpw (CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  setDiff () 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Sets  ctrlw  for  differential  mode  and  writes  ctrlw  to 

//  A2D  Control  Register 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  initHardware ( ) 
//***************************************** 

void  a2dClass :: setDiff (void) 

{ 

ctrlw     |=    DIFF; 
outpw (CNTL, ctrlw) ; 

} 
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//it********************************* 

//  FUNCTION  NAME:  setChannel ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Loads  sequencer  memory  with  channel  data 

/ /  CALLS :  progSeq ( ) ,  outpw ( ) ,  runSeq ( ) 

//  CALLED  BY:  initHardware ( ) 

//  VARIABLES:  seq  -  sequencer  number 

//  ch   -  channel  number 

//  glO  -  xlO  gain  value 

//  g2   -  x2  gain  value 

//it******************************************************************** 

void  a2dClass :: setChannel (unsigned  seq, unsigned  ch, unsigned  glO, unsigned  g2i 
{ 

unsigned  d  =  0 ; 

setProgSeq( ) ;  //  set  sequencer  program  mode 

outpw (COUNT, seq) ;        //  set  sequencer  address 

//load  sequencer  memory 

d  |=  ch<<8;  //  channel 

d  |=  (g2<<12);  //  gain  X2 

d  1=  (gl0«14);  //  gain  XlO 

outpw (MEM, d) ;  //  load  sequencer 

setRunSeq ( ) ;  //  set  sequencer  run  mode 

} 

//    FUNCTION  NAME:  setProgSegO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  19  9  6 

//  DESCRIPTION:  Sets  sequencer  to  program  mode 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  setChannel  (  ) 
//************••***•**************•* 

void  a2dClass : : setProgSeq (void) 
{ 

ctrlw  |=  PRG_SEQ; 

outpw (CNTL, ctrlw) ; 

} 
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//  FUNCTION  NAME:  setRunSeqO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Sets  sequencer  to  run  mode 

//  RETURNS:  void 

//  CALLS:  outpw() 

//  CALLED  BY:  none 
//**•******************************•**•*** 

void  a2dClass : : setRunSeq ( void) 
{ 

ctrlw  &=  ~PRG_SEQ; 

outpw(CNTL, ctrlw) ; 

} 

//•it********************************* 

//  FUNCTION  NAME:  setCount ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Loads  sequencer  address  counter  with  number  of  channels 

//  to  scan. 

//  RETURNS:  void 

//  CALLS:  outpw(),  setProgSeq ( ) ,  setRunSeq() 

//  CALLED  BY:  initHardware ( ) 

//  VARIABLES:  nch  -  number  of  channels  to  sequence 

void  a2dClass :: setCount (unsigned  nch) 

{ 

nch=nch<<4;  //  put  in  upper  nibble 

outpw (COUNT, nch) ;  //  out  to  register 

setProgSeq ( ) ;  //  reset  sequencer 

setRunSeq ( ) ;  //  put  it  in  run  mode 

} 

//  FUNCTION  NAME:  setAcDc ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  AC  or  DC  Coupling 

//  RETURNS:  void 

//  CALLS:  outpw () 

//  CALLED  BY:  initHardware ( ) 

//  VARIABLES:  acdc  -  holds  coupling  value 
//**********************•************* 

void  a2dClass :: setAcDc (unsigned  acdc) 
{ 

if  (acdc) 

ctrlw  |=  ACDC;       //  acdc=l  ->  DC 
else 

ctrlw  &=  -ACDC;      //  acdc=0  ->  AC 
outpw (CNTL, ctrlw) ; 

) 
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/  /*  +  *************************************  +  ********  +  +  **  +  ***  +  +  *********** 

//  FUNCTION  NAME:  lockTrigger ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  19  9  6 

//  DESCRIPTION:  Prevents  triggering 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  initSampler ( ) 
//************************************* 

void  a2dClass :: lockTrigger (void) 

{ 

ctrlw  &=  ~RST_TRG; 
outpw (CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  unlockTrigger ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Allow  the  trigger  to  function 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  readSmaple ( ) 
//***************************************^ 

void  a2dClass: : unlockTrigger ( void) 
{ 

ctrlw  |=  RST_TRG|SET_TRG; 

outpw (CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  setTrigger ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Toggle  the  trigger  (software  triggering) 

//  RETURNS:  void 

//  CALLS:  outpw () 

//  CALLED  BY:readSample( ) 
//*************************************** 

void  a2dClass :: setTrigger (void) 
{ 

outpw (CNTL, ctrlw&~SET_TRG|RST_TRG) ; 

outpw (CNTL, ctrlw |  SET_TRG I RST_TRG) ; 

} 
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//  FUNCTION  NAME:  resetTrigger ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Clears  the  trigger 

//  RETURNS:  void 

//  CALLS:  outpw() 

//  CALLED  BY:  readSample ( ) 

//***+***************************************************************** 

void  a2dClass: : resetTrigger (void) 
{ 

outpw(CNTL, ctrlw |SET_TRG&~RST_TRG) ; 

outpw (CNTL, ctrlw |SET_TRG|  RST_TRG)  ; 

} 

//it******************************************************************** 

//    FUNCTION  NAME:  setRmsOnf) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Switches  in  the  RMS  measurement  chip 

//  RETURNS:  void 

/ /  CALLS :  OUtpw ( ) 

//  CALLED  BY:  none 
//it******************************************************************** 

void  a2dClass : : setRmsOn (void) 

{ 

ctrlw  |=  RMS; 
outpw (CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  setRmsOffO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  19  9  6 

//  DESCRIPTION:  Switches  out  RMS  measurement  chip 

//  RETURNS:  void 

//  CALLS:  outpw () 

//  CALLED  BY:  initSampler ( ) 
//********************************************************************* 

void  a2dClass : : setRmsOf f (void) 
{ 

ctrlw  &=  -RMS; 

outpw (CNTL, ctrlw) ; 

} 
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//********************************************************************* 

//  FUNCTION  NAME:  setSequencer ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  the  A2D  module  to  sequencer  mode 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  none 
//********************************************************************* 

void  a2dClass : : setSequencer (void) 
{ 

ctrlw  |=  SAM_SEQ; 

outpw ( CNTL, ctrlw) ; 

} 

//********************************************************************* 

II    FUNCTION  NAME:  setSamplerRate ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Sets  the  A2D  module  to  sampler  mode 

//  RETURNS:  void 

//  CALLS:  outpw () 

//  CALLED  BY:  none 

//  VARIABLES:  rate  -  sampler  rate 

void  a2dClass :: setSamplerRate (unsigned  rate) 
{ 

ctrlw  &=  ~SAM_SEQ;    //Set  to  sampler  mode 

ctrlw  &=  CLRRATE;     //Clear  previous  rate  to  000 

ctrlw  1=  rate;        //Set  new  rate 

outpw (CNTL, ctrlw) ;    //Set  Control  Word 

} 

//************* *.*  ****************************************************** 

//  FUNCTION  NAME:  gateloutOnO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Set  GATE10UT  bit  of  control  word  high 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  none 
//********************************************************************* 

void  a2dClass : :gateloutOn ( void) 
{ 

ctrlw  |=  GATE10UT; 

outpw (CNTL, ctrlw) ; 

} 
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//  FUNCTION  NAME:  gateloutOf f ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Set  GATEIOUT  bit  of  control  word  low 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  none 
//************************************************************■********* 

void  a2dClass : :gateloutOf f (void) 

{ 

ctrlw  &=  -GATEIOUT ; 
outpw (CNTL,  ctrlw)  ; 

} 

//  FUNCTION  NAME:  squareWaveTimerl ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  timer  channel  1  to  square-wave  input 

//  RETURNS:  void 

//  CALLS:  outp() 

//  CALLED  BY:  none 

//  VARIABLES:  dt-micro  seconds  per  period  (1  to  8192) 

//  assuming  8  MHz  clock  input 

//  ch-timer  channel  1 

//  ph-local  variable 

//  pl-local  variable 

//******•*****************************+ 

void  a2dClass :: squareWaveTimerl (unsigned  dt ) 
( 

char     ph,pl; 

pi  =  (dt*8)&0xFF;         //  8  CLOCKS  PER  uS 
ph  =  (dt*8)»8; 

outp(TIMERC, 0x7  6) ;       //  initialize  timer 
outp(TIMERl,pl) ;         //  dt  uS  delay 
outp(TIMERl,ph) ;         //  with  8  MHz  clock 
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//  FUNCTION  NAME:  initTiming () 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Initialize  the  A2D  timing  using  timer  2 

//  RETURNS:  void 

//  CALLS:  outp() 

//  CALLED  BY:  initHardware ( ) 

//  VARIABLES:  dt  -  number  of  micro  seconds  (3  to  273  0) 

//**************************************************+*+*******+******** 

void  a2dClass :: initTiming (unsigned  dt ) 
{ 

char     ph,pl; 

pi  =  (dt*8)&0xFF;     //  8  CLOCKS  PER  uS 
ph  =  (dt*8)>>8; 

outp(TIMERC, 0xB6) ;    //  initialize  timer2 
outp(TIMER2,pl) ;      //  dt  uS  delay 
outp(TIMER2,ph) ;       //  with  8  MHz  clock 

} 

//a-**************************************** 

//  FUNCTION  NAME:  resetFifo() 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Rewind  FIFO  to  beginning  of  memory 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  none 
//•it************************************ 

void  a2dClass : : resetFifo (void) 
{ 

ctrlw  &=  ~RST_FIFO; 

outpw (CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  setFifo() 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Enable  FIFO  to  acquire  data 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  initSampler ( ) 
//lit******************************************* 

void  a2dClass : : setFifo (void) 
{ 

ctrlw  |=  RST_FIFO; 

outpw (CNTL, ctrlw) ; 

} 
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//*****************************************************  ^^^^^^.k^^ 

//    FUNCTION  NAME:  getFif oStatus ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Returns  FIFO  status 

//  RETURNS:  RETURNS:  6  -  empty 

//  5  -  half  full 

//  1  -  full 

//  CALLS:  inpw() 

//  CALLED  BY:  readSample ( ) 
//********************************************** *********************** 

unsigned  a2dClass : : getFif oStatus (void) 
{ 

return  ( inpw(STAT) &7 ) ; 

} 

//****************************************************** *************** 

//  FUNCTION  NAME:  getFif oData ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Returns  next  data  word  stored  in  FIFO 

//  RETURNS:  16bits  of  data.  Lower  12  are  A2D  data 

//  CALLS:  inpw() 

//  CALLED  BY:  readSample ( ) 

//*************  **************************************************^^^ 

signed  a2dClass: :getFifoData (void) 
{ 

return  ( inpw(FIFO) &0x0FFF) ;      //Get  data  and  mask  upper  nibble 

} 

//******************  +  +  *  +  ***********  +  +  +  *  +  +  +  +  *ir  +  *ir  +  +  _kiT  +  irir  +  +  ir  +  icieir^ieir^_kii_k^_k 

//    FUNCTION  NAME:  setlntRatef) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Program  timer  channel  0  to  set  the  desired  interrupt 

//  rate 

//  RETURNS:  void 

//  CALLS:  outp() 

//  CALLED  BY:  none 

//  VARIABLES:  intrate-micro  sees  per  period  (1  to  8192) 

//  assuming  8  MHz  clock  input 

void  a2dClass: :setIntRate (unsigned  intrate) 

{ 

outp(TIMERC,0x36) ;  //  set  timer  0  to  mode  3 

outp(TIMERO, (intrate*8)&0xFF) ;  //  Load  Least  Significant  Byte 

outp(TIMERO,  (intrate*8)»8)  ;  //  Load  Most  Significant  Byte 

) 
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//••it**************************** 

//  FUNCTION  NAME:  intOf f ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Locksout  the  interupt  request  line 

//  RETURNS:  void 

//  CALLS:  outpw() 

//  CALLED  BY:  none 
//********************************************* 

void  a2dClass : : intOf f (void) 
{ 

ctrlw  &=  ~INT_EN;         //  INT_EN  is  active  high 

outpw(CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  intOn ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Enables  system  interuppt  request 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  none 
//*************•********+*****+******* 

void  a2dClass : : intOn (void) 
{ 

ctrlw  |=  INT_EN;         //  INT_EN  is  active  high 

outpw (CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  setTriggerLevel ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  the  trigger  level 

//  RETURNS:  void 

/ /  CALLS :  outp ( ) 

//  CALLED  BY:  none 

//  VARIABLES:  tl-trigger  level   (0=-10V,  128=0V,  255=+10V) 
//it******************************************************************** 

void  a2dClass :: setTriggerLevel (unsigned  tl) 
{ 

outp(DAC, tl) ; 

} 
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//  FUNCTION  NAME:  setTriggerPosit ion ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  falling  or  rising  edge  trigger 

//  RETURNS:  void 

/ /  CALLS :  outpw { ) 

//  CALLED  BY:  none 

//  VARIABLES:  tp :  0=falling,  l=rising 

void  a2dClass :: setTriggerPosition (unsigned  tp) 

{ 

ctrlw  &=  ~TRG_POS;        //Clear  previous  TRG_POS 
ctrlw  |=  (tp) ?TRG_POS:0;  //Evaluate  tp  and  set  ctrlw 
outp(CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  zeroOf f set ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Calibrates  zero  offset  error 

//  RETURNS:  void 

//  CALLS:  outpw () 

//  CALLED  BY:  none 
//♦it********************************* 

void  a2dClass :: zeroOff set (void) 
{ 

unsigned  d=0 , i, g2 , glO ; 

float     sum; 

float     of fsetErr [4] [4] ; 

float     bits [4] [4]  ; 

unsigned  gainslO [4]  =  {1,10,100,100}; 

unsigned  gains2[4]   =  {1,  2,   4,   8}; 

clrscr ( ) ; 

printf ( " \n\tG10\tG2\t  OFFSET\t\t  BITS"); 

for (glO=0;glO<4;glO++) 
f or (g2=0 ;g2<4 ;g2++ ) 

printf (M\n\t%d\t%d\t+X.XXXXXX\t+XX.X",glO,g2) ; 

setRmsOf f ( ) ; 

setAcDc (0) ; 

setSequencer ( ) ; 

initTiming (3) ; 

setChannel (0, 0,gl0,g2) ; 

grndlnput ( ) ; 

delay(5);  //Let  new  gain  values  stabilize 

while ( Ikbhit () ) { 

for(glO=0;glO<4;glO++) { 
for (g2=0;g2<4;g2++) { 
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setChannel (0, 0,gl0,g2) ; 

grndlnput ( ) ; 
lockTrigger ( ) ; 
resetFifo ( ) ; 
setFifo( ) ; 
unlockTrigger ( ) ; 
setTrigger ( ) ; 
delay  (1)  ; 

while (getFifoStatus  (  )  ! =FULL)  ; 
lockTrigger ( ) ; 

for (i=0, sum=0 . 0 ; i<FIFOSIZE; i++ ) { 

d=getFifoData ( ) ; 

sum+= ( float )d* 10/2048; 
} 
offsetErr [glO]  [g2]  =  (  (float)  ( sum/FIFOSIZE) -10)  / 

(float) (gains 10 [glO] *gains2 [g2] )  ; 

bits[gl0] [g2]=(float) (of f setErr [glO ] [g2]*4096/ 

2  0*gainsl0 [glO] *gains2[g2] ) ; 
} 

clrscr  (  )  ; 

printf ( "\n\tG10\tG2\t  OFFSET\t\t  BITS"); 
for (glO=0;glO<4;glO++) { 
for (g2=0;g2<4;g2++) { 

printf (" \n\t%d\t%d\t%+1.6f\t%+04. If ",gl0,g2, 
offsetErr[glO] [g2] fbits[gl0] [g2] ) ; 
} 

} 
} 

f reelnput ( ) ; 
getch ( ) ; 

} 

} 

//  FUNCTION  NAME:  grndlnput ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Grounds  the  two  diff  input  for  zero  adjust 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  none 
//************************************************* 

void  a2dClass :: grndlnput (void) 
( 

ctrlw  |=  CAL; 

outpw (CNTL, ctrlw) ; 

} 
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//•it*********************************************** 

//  FUNCTION  NAME:  freelnput  (  ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  199  6 

//  DESCRIPTION:  Ungrounds  the  two  diff  inputs 

//  RETURNS:  void 

/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  none 
//**************************************** 

void  a2dClass :: freelnput (void) 
{ 

ctrlw  &=  -CAL; 

outpw (CNTL, ctrlw) ; 

} 

//  FUNCTION  NAME:  zeroAdjust ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95] 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Adjust  the  trimmer  on  the  PGA 

//  RETURNS:  void 

//  CALLS:  outpw () 

//  CALLED  BY:  none 
//•••it**************************** 

void  a2dClass :: zeroAdjust (void) 
{ 

int      i  ; 

unsigned  d; 

float     sum, of f setErr ; 

clrscr  (  )  ; 

printf ( "\n\nADJUST  THE  TRIM  POT  FOR  0.0  OFFSET\n\n" ) ; 

setRmsOf f ( ) ; 
setAcDc (0) ; 
setSequencer  (  )  ,- 
initTiming (3 ) ; 

while ( ikbhit ( ) ) { 

setChannel (0,0,3,3) ; 

grndlnput ( ) ; 

lockTrigger ( ) ; 

resetFifo ( ) ; 

setFifo( ) ; 

unlockTrigger ( ) ; 

setTrigger ( ) ; 

while (getFif oStatus ( ) ! =FULL) ; 

lockTrigger ( ) ; 

for (i=0, sum=0 .0; i<FIFOSIZE; i++) { 

d=getFifoData( ) ; 

sum+= (float ) d* 10/2 048; 
} 
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offsetErr=( (float) ( sum/FIFOSIZE) -10 ) / 80 0  0.0; 

printf ( "\tTHE  MEASURED  DC  OFFSET  IS:  %+8 . 6f \r " , of f setErr ) ; 


} 


f reelnput ( ) ; 
getch( ) ; 
} 

P.  A2D.CFG 

8  ; seqcnt : number_of_seq_addresses_to_load 

0  ;mode_sel: DIFF=1 SE=0 

1  ;mode_acdc :_Signal_coupling_select DC=1 AC=0 

8  ; chcnt : Number_of_channels_to_sequence_ (hex, _1-F) 

3125  ;delta_t : Chan_to_Chan_Sample_rate_in_microsecs_3 -8192_=_1/Hz*chcnt 

7  ; samprate : Sample_rate_in_recurrent_mode 0 ( fast ) -7 (slow) 

0  ; sampindex:_Which_channel_to_sample_in_recurrent_mode 

0  0  0  0 

110  0 

2  2  0  0 

3  3  0  0 

4  4  0  0 

5  5  0  0 

6  6  0  0 

7  7  0  0 

8  8  0  0 

9  A  2  0 
A  5  2  0 
B  A  2  0 
C  5  2  0 
D  A  2  0 
E  5  2  0 
F  A                   2  0 
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APPENDIX  B:  Serial  Port  Communications  Source  Code  (C++) 
A.  GLOBALS.H 

#ifndef  GLOBALS_H 

#define  GLOBALS_H 

//  types 

typedef   unsigned  char  BYTE; 
typedef   unsigned  short  WORD; 
typedef   unsigned  long   DWORD; 

#define   MEM(seg,ofs)      (*((BYTE  f ar* ) MK_FP ( seg, of s ) ) ) 
#define   MEMW (seg, of s )    (*((WORD  f ar* ) MK_FP ( seg, of s ) ) ) 

enum  Boolean    {FALSE,  TRUE} ; 

//  basic  bit  twiddles 

#define   set  (bit)  (l«bit) 

#define   setb(data, bit )  data  I  set (bit) 

#define   clrb (data, bit )  data  &  !set(bit) 

#define   setbit (data, bit )  data  =  setb (data, bit ) 

#define   clrbit (data, bit )  data  =  clrb (data, bit ) 

//  specific  to  ports 

#def ine   setportbit (reg, bit )   outportb(reg, setb ( inportb(reg) , bit )  ) 

#def ine   clrportbit (reg, bit)   outportb (reg, clrb ( inportb (reg) , bit ) ) 

#endif 
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B.  BUFFER.H 

#ifndef  BYTEBUFFER_H 

#define  BYTEBUFFER_H 

#include  "toetypes.h" 

#define   BYTEBUFSIZE     32 

/**************************************************************************** 

CLASS: Buffer 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  19  9  5 

FUNCTION: Base  class  for  use  as  a  polymorphic  reference  in  the  serial  port  code 

which  defines  a  buffer  to  be  used  in  serial  port  communications. 
*********************************************************** 

class  Buffer   { 

public : 

/ /Constructor 

Buffer(WORD  sz)  :  getPtr(O),  putPtr(O),  size(sz)   {} 

//Checks  for  the  arrival  of  new  characters  in  the  buffer 
virtual  Boolean   hasData ( ) {  return  Boolean (putPtr  != 

getPtr) ;  } 

//How  much  of  the  Buffer  is  used  (rounded  percentage 

//0  -  100) 

virtual  int       capacityUsed ( ) ; 

//Read  from  the  buffer 

virtual  Boolean    Get  (BYTES, )  =  0  ; 

//Read  to  the  buffer 

virtual  void       Add (BYTE)  =  0; 

protected: 

//Increment  the  pointer  to  next  position 

void   inc (WORD&  index)  {  if  (++index  ==  size)  index  =0;  } 

//Decrement  the  pointer 
WORD  before (WORD  index) {  return  ((index  ==  0)  ?  size  -  1  : 

index  -  1 ) ; } 

WORD     getPtr,  //Location  of  unread  data 

putPtr,   //Location  to  read  data  to 
size;     //Size  of  the  buffer  in  bytes 
}; 
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/****************************************************■***■*.*.**.*..*..*.*..*.* 

Defines  a  single  buffer  of  a  specified  size  for  buffering  charaters 
received  via  serial  port. 
***************************************************** 

class  byteBuffer  :  protected  Buffer  { 

public : 

byteBuffer (BYTE  sz=BYTEBUFSIZE) ; 
-byteBuf fer ( )      {  delete  []  buf;  } 

Buffer: :hasData; 

Buffer: :capacityUsed; 

//  buffer  extraction 
Boolean      Get (BYTE&) ; 

//  buffer  insertion 
void      Add (BYTE  ch) ; 
void      Add(const  char*); 

byteBuf fer&   operator  +=  (BYTE  ch) {  Add(ch) ;  return  *this; 

} 

protected: 

BYTE*  buf; 

}; 
#endif 


C.  BUFFER.CPP 

#include  <iostream.h> 
#include  <stdio.h> 

#include  "globals.h" 
#include  "buffer.h" 

//******************************************** 

//  Buffer 
//******************************************** 

//  Returns  the  percentage  of  the  buffer  in  use 

int 

Buffer: : capacityUsed ( ) 

{ 

int  cap  =  (putPtr  +  size)  %  size  -  getPtr; 

return  100  *  cap  -/  size; 
} 
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//  byteBuffer 
//*******•****************************•******■*** 

//Constructor,  instantiates  a  buffer 
byteBuf fer : :byteBuf fer  (BYTE  sz)  :  Buffer (sz) 

{ 

buf  =  new  BYTE[size]; 

} 

//Reads  a  charcter  from  the  buffer 

Boolean 

byteBuffer: : Get (BYTE&  data) 

{ 

if  (hasData( ) )   { 

data  =  buftgetPtr]; 
inc (getPtr ) ; 

return  TRUE; 

} 

return  FALSE; 
} 

//Writes  a  character  to  the  buffer  and  checks  for  buffer  overflow 

void 

byteBuffer: : Add (BYTE  ch) 

{ 

buf[putPtr]  =  ch; 

inc (putPtr ) ; 

if  (!hasData())   {    //  if  there's  no  data  after  adding  data, 

//  it  overflowed 
cerr  <<  "\nError:  byteBuffer  overf low\n" ; 

} 
} 

//Writes  a  character  to  the  buffer 

void 

byteBuf fer :: Add (const  char*  s) 

{ 

while  (*s) 

Add(*s  +  +)  ; 
} 
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D.  GPSBUFF.H 


ifndef 
#def ine 


_GPSBUFF_H 
GPSBUFF  H 


#include  "buffer.h" 
#include  "toetypes.h" 

#define   GPSBLOCKS       4 
#define   LINE_FEED       10 
#define   CARR  RETURN     13 


/IT********************************************************-******** 

Class  buffers  GPS  position  messages  via  serial  port  communications. 

Uses  a  multiple  buffer  system  in  which  each  buffer  is  capable  of  holding  a 
single  position  message.  Buffers  are  filled  and  processed  sequentially  in  a 
round  robin  fashion.  Messages  are  checked  for  validity  only  upon  attempted 
reads  from  the  buffer. 


**************************************************************** 


/ 


class  GPSbuffer  :  public  Buffer   { 

public : 

GPSbuffer (BYTE  GPSblocks=GPSBLOCKS) ; 
-GPSbuffer () {delete  []  block;} 


Boolean 
Boolean 
Boolean 
void 


hasData ( ) ; 
Get(BYTESc) 
Get (GPSdata) 
Add (BYTE  ch) 


//a  complete  structure  is  ready 
{return  FALSE; } 

//  get  a  complete  structure  filled  in 
//  build  the  structure  as  each  byte 
//  is  added 


protected: 

Boolean   validHeader (GPSdata) ;  //  check  a  block  for  valid 

//  header 
GPSdata   *block;      //  hold  the  buffered  GPS  data 
WORD     current,     //  the  current  GPS  block  in  use 
last;        //  the  last  GPS  block  in  use 


BYTE 
}; 

#endif 


^putPlace;   //  place  to  put  the  next  charater  received 
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E.  GPSBUFF.CPP 

#include  <iostream.h> 
#include  <stdio.h> 

# include  "gpsbuff.h" 

/**************************************************************** 

PROGRAM :GPSbuffer  (Constructor) 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Allocates  message  buffers,  indicate  that  no  data  has 

beenreceived  by  equalizing  current  and  last  and  set  position 

into  which  initial  character  will  be  read. 

RETURNS : nothing . 

CALLED  BY: navigator  class  (nav.h) 

CALLS :none . 
************************************************************** 


GPSbuffer: :GPSbuffer (BYTE  GPSblocks)  : 
current (0) ,  last (0) , 

Buff er (GPSblocks )  //  Call  to  base  class  constructor 
{ 

block  =  new  GPSdata [GPSblocks ] ;   //Create  an  array  of  GPSdata 

//elements 
putPlace  =  & (block [current ][ 0 ]) ;  //Set  the  place  for  the 

//  first  character 
} 
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PROGRAM :GPSbuffer: :Add 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Interrupt  driven  routine  which  writes  incoming  characters  into  the 

gps  buffers 

RETURNS : nothing . 

CALLED  BY:   interupt  driven  by  buf f eredSerialPort 

CALLS : none . 

void 

GPSbuffer: : Add (BYTE  data) { 

static  BYTE  lastChar (data) ;   //Holds  last  for  <cr>  <lf>  detection 
static  Boolean  IfFlag  =  FALSE;  //True  when  message  end  is  detected 

//Is  a  new  message  starting? 
if  (IfFlag  &&  (data  ==  ' @ ' ) )  { 

last  =  current;   //  Set  last  to  buffer  with  newest  message. 

inc (current ) ;     //  Set  current  to  the  next  buffer 

//  Set  putPlace  to  the  beginning  of  the  next  buffer. 

putPlace  =  & (block [current ]  [0 ])  ; 

IfFlag  =  FALSE;   //  reset  for  end  of  next  message. 
} 

*putPlace++  =  data;//  Write  character  into  the  buffer. 

//Has  the  end  of  a  message  been  received? 
if  ((lastChar  ==  CARR_RETURN)  && 

(data  ==  LINE_FEED) )  ( 

IfFlag  =  TRUE; 
} 
lastChar  =  data;  //Save  last  character  for  <cr>  <lf>  detection 
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/**************************************************************** 

PROGRAM :GPSbuffer: :Get 
AUTHOR: Eric  Bachmann,  Dave  Gay 
DATE: 11  July  199  5 
FUNCTION: Checks  to  see  if  a 

new  message  has  arrived,  copies  it  into  the 

input  argument  data  and  returns  a  flag  to  indicate  whether  a 

newmessage  was  received. 

RETURNS : TRUE ,  if  a  new  valid  position  has  been  received. 

FALSE,  otherwise 

CALLED  BY:navPosit  (nav.cpp) 

initializeNavigator  (nav.cpp) 

CALLS :GPSbuffer: :hasData 
*********************************************************** 

Boolean 

GPSbuf fer: :Get (GPSdata  data) 

{ 

//  Has  a  new  valid  message  been  received, 
if  (hasData( ) )   { 

//  Copy  the  message  out  of  the  buffer, 
memcpy  (data,  block  +  last,  GPSBLOCKSIZE) ; 
//  Indicate  that  this  message  has  been  read, 
last  =  current; 
return  TRUE; 
} 
else  { 

return  FALSE; 
} 
} 
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PROGRAM :GPSbuffer: :hasData 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Determines  whether  a  new  message  has  been  received  and 

checks  to  see  if  it  has  a  valid  header. 
RETURNS : TRUE ,  if  a  new  valid  message  has  been  received. 
CALLED  BY:    GPSbuf f er : : Get  (buffer. cpp) 
CALLS:        validHeader  (buffer. cpp) 

Boolean 

GPSbuf fer: :hasData( ) 

{ 

//  Has  a  new  message  with  a  valid  header  been  received 
if  (last  !=  current)  { 

if  (validHeader(block[last] ) )  { 

return  TRUE; 
} 
else  { 

return  FALSE; 
} 
} 

return  FALSE; 
} 

PROGRAM : val idHeader 

AUTHOR: Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Checks  to  see  if  a  message  has  the  proper  header  for  a 

Motorola  position  message.  (@@Ea) 
RETURNS : TRUE ,  if  the  header  is  valid.  FALSE,  otherwise. 
CALLED  BY:GPSbuffer: :hasData  (buffer. cpp) 
CALLS: none. 
COMMENTS : 

Boolean 

GPSbuf fer: : validHeader (GPSdata  dataPtr) 

{ 

if  ((dataPtr[0]  ==  '§')  &&  (dataPtrfl]  ==  '©')  && 

(dataPtr[2]  ==  'E')  && 
(dataPtr [3]  ==  'a' ) )  { 
return  TRUE; 
} 
else  { 

return  FALSE; 
} 
} 
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F.  PORTBANK.H 

#ifndef  PORTBANK_H 

#define  PORTBANK_H 

#include  "serial. h" 
#include  "buffer.h" 

/•♦••••it********************************* 

CLASS :portBank 

AUTHOR:Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Manages  up  to  four  buf f erdSerialPort  instances. 
it***************************************************************/ 

class   portBank   { 

public : 

portBank ( ) ; 

-portBank ()     {  cleanup ();  } 

buf feredSerialPort&   Init(COMport  portnum,  BYTE  irq, 
BaudRate, Par ityType, BYTE  wordlen,  BYTE  stopbits, 
handshake,  Buffers) ; 

void   cleanupO  ; 

friend  IntHandlerType  COMlhandler,  C0M2handler, 
C0M3handler , C0M4handler ; 

protected : 

buf f eredSerialPort*   ports [4]; 

}; 

extern  portBank    COMports; 

#endif 
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G.  PORTBANK.CPP 

#include  <iostream.h> 

#include  "serial. h" 
#include  "buffer. h" 
#include  "portbank.h" 

portBank     COMports; 

//  Constructor,  sets  up  array  of  ports 

portBank : : portBank ( ) 

{ 

for  (int  i  =  0 ;  i  <  4 ;  i  +  +) 
ports [ i]  =  0 ; 
} 

//  Resets  all  ports  to  the  original  parameters 

void 

portBank : : cleanup ( ) 

{ 

for  (int  i=0;  i<4;  i++) 
delete  ports [i]; 
} 

//  Initializes  a  serial  port  based  up  the  input  arguments 

buf feredSeria 1 Port & 

portBank :: Init (COMport  portnum,  BYTE  irq,  BaudRate  baud,  ParityType  parity, 

BYTE  wordlen,  BYTE  stopbits,  handshake  shake,  Buffer&  buf) 
{ 

int    index  =  BYTE (portnum)  -  1; 
if  (ports [ index] ) 

delete  ports [index] ; 
ports [index]  =  new  buf feredSerialPort (portnum,  irq,  baud, 

parity , wordlen,  stopbits,  shake,  buf); 
return  *ports [index] ; 
} 

//  Three  specific  interrupt  handlers  which  map  each  interupt  to  ///the 

proper  ISR. 

void 

interrupt 

COMlhandler(  .  .  .  ) 

{ 

COMports .ports [ 0 ] ->processInterrupt ( ) ; 

EOI; 
} 

void 

interrupt 
C0M2handler(  .  .  .  ) 
{ 

COMports .ports [1] ->processInterrupt ( ) ; 

EOI; 
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} 


void 

interrupt 

COM3 handler  (...) 

{ 

COMports .ports [2 ] ->processInterrupt ( ) ; 

EOI; 
} 

void 

interrupt 
C0M4handler ( . . . ) 
{ 

COMports . ports [ 3 ] ->processInterrupt ( ) ; 

EOI; 
} 


H.  SERIAL.H 


#ifndef 
#def ine 


_SERIAL_H 
SERIAL  H 


#include  <dos.h> 

#include  <stdio.h> 

# include  "globals.h" 

#include  "buffer. h" 


//  user  defines 

#define      ALMOST  FULL 


80 


// 


full  to  turn  off  DTR 


//  leave  the  following  alone  -  hardware  specific 


enum  COMport 

enuiti  BaudRate 

enum  ParityType 

enum  handshake 

enum  Shake 

enum  interruptType 


{C0M1=1,  COM2,  COM3,  COM4}; 

{b300,  bl200,  b2400,  b4800,  b9600}; 

{ERROR=-l,  NOPARITY,  ODD,  EVEN}; 

{ NONE ,  RTS_CTS ,  XON_XOFF } ; 

{off,  on}; 

{rx_rdy,  tx_rdy,  line_stat,  modem_stat } ; 


#def ine 
#def ine 
#def ine 
#def ine 


BIOSMEMSEG 
DLAB 
IRQPORT 
EOI 


0x40 
0x80 
0x21 
outportb ( 0x2  0 , 0x2  0  [ 


#define  COMlbase 

#define  COM2base 

#define  COM3base 

#define  COM4base 


MEMW( BIOSMEMSEG, 0) 
MEMW ( BIOSMEMSEG , 2 ) 
0x03e8 
0x02e8 


#define  TX 
ttdefine  RX 
#define   IER 


(portBase) 
(portBase) 
(portBase+1 ) 
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#define  IIR      (portBase+2) 

#define  LCR      (portBase+3) 

#define  MCR      (portBase+4) 

#define  LSR      (portBase+5) 

#define  MSR      (portBase+6) 

#define  LO_LATCH  (portBase) 

#define  HI_LATCH  (portBase+1) 

/****************************************************  +  *******■*■*■*.* 

CLASS : serialPort 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:   Defines  a  simple  serial  port. 

class  serialPort   { 

public : 

serialPort (COMport  port,  BaudRate,  ParityType,  BYTE  wordlen, 
BYTE  stopbits,  handshake) ; 

Boolean      Send (BYTE  data) ; 
Boolean      Get(BYTE&  data); 

inline  Boolean  dataReady ( ) ; 
Boolean  statusChanged ( ) 

{  return  Boolean (( ifportbit (MSR, 0 )  I  I  ifportbit (MSR, 1 ) )  )  ;  } 


//  the  rest  are  only  if  handshake  is  specified  as  RTS_CTS 

{  return  ifportbit (MSR, 4) ;  } 
{  return  ifportbit (MSR, 5) ;  } 

(  setportbit (MCR, 0) ;  } 
{  clrportbit (MCR, 0) ;  } 


{  setportbit (MCR, 1) ;  } 
{  clrportbit (MCR, 1) ;  } 


Boolean 

isCTSon( ) 

Boolean 

isDSRon( ) 

void 

setDTRon( ) 

void 

setDTRof f ( ) 

void 

toggleDTR( ) ; 

void 

setRTSon( ) 

void 

setRTSoff () 

void 

toggleRTS ( ) ; 

protected: 

WORD 

portBase; 

handshake 

ShakeType ; 

Shake 

DTRstate, 

RTSstate; 

}; 


inline  Boolean     ifportbit (WORD,  BYTE); 
inline  void        toggle (Shake&) ; 
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//  this  is  the  type  for  a  standard  interrupt  handler 
typedef  void  interrupt  ( IntHandlerType)  (  .  .  .  )  ■ 

/**************************************************************** 

CLASS :buf feredSerialPort 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 
DATE: 11  July  1995 
FUNCTION:   Defines  a  buffered  serial  port  which  is  interrupt  driven 
on  receive,  and  buffers  all  incoming  characters  in  the  specified  buffer 
************************************************************* 

class  buf feredSerialPort  :  public  serialPort   { 

public : 

buf feredSerialPort (COMport  portnum,  BYTE  irq,  BaudRate, 
ParityType,  BYTE  wordlen,  BYTE  stopbits,  handshake,  Buffer^  )  ,- 

-buf feredSerialPort ( ) ; 

Boolean      Get(BYTE&  data);      //  buffered  version 

protected : 

Buffers      buf; 

BYTE  irqbit, //Value  to  allow  enable  PIC  interrupts  for  COM  port 
origirq, //keep  the  original  value  of  the  8259  mask  register 
comint ; 

void   processlnterrupt ( ) ;      //  buffers  the  incoming  character 

IntHandlerType  *origcomint ; //  keep  the  original  vector  for 

//restoring  later 

//  this  allows  the  actual  handlers  to  access  processlnterrupt ( ) 
friend  IntHandlerType  COMlhandler,  C0M2handler,  C0M3handler, 

C0M4handler; 

}; 
#endif 
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I.  SERIAL.CPP 

#include  <iostream.h> 
#include  <stdio.h> 
#include  "serial. h" 

/**************************************************************+* 

Usage  Note:  Because  of  the  interrupt  handlers  used,  you  MUST  call  your 
buf f eredSerialPort  objects  portl,  port2,  or  port3,  so  the 

right  handler  gets  called  and  can  properly  service  the  interrupt. 
*************************************************************** 

/**************************************************************** 

PROGRAM:  serialPort  (Constructor) 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: 

Initializes  the  one  of  the  Serial  Ports. 

1)  Determines  the  base  I/O  port  address  for  the  given  COM  port 

2)  Sets  the  8259  IRQ  mask  value 

3)  Initializes  the  port  parameters  -  baud,  parity,  etc. 

4)  Calls  the  routine  to  initialize  interrupt  handling 

5)  Enables  DTR  and  RTS,  indicating  ready  to  go 
****************************************************************/ 

serialPort :: serialPort (COMport  port,   BaudRate  speed,   ParityType  parity, BYTE 
wordlen,  BYTE  stopbits,  handshake  hs)  ': 

DTRstate(of f ) ,  RTSstate (of f ) ,  ShakeType (hs ) 

{ 

switch  (port)   { 

case  C0M1 :   portBase  =  COMlbase; 

break; 
case  COM2:   portBase  -    C0M2base; 

break; 
case  COM3:   portBase  =  C0M3base; 

break; 
case  COM4:   portBase  =  C0M4base; 
break; 
}   //switch 

const  WORD      bauddiv[]  =  {0x180,  0x60,  0x3  0,  0x18,  OxC}; 

//  Change  1 
outportb(IER, 0) ;        //  disable  UART  interrupts 

(void) inportb(LSR) 

(void) inportb(MSR) 

(void) inportb( IIR) 

(void) inportb(RX) ; 

outportb(LCR, DLAB) ; //set  DLAB  so  can  set  baud  rate(read  only 

//  port) 
outportb(LO_LATCH,bauddiv[ speed]  &  OxFF) ; 
outportb(HI_LATCH, (bauddiv[ speed]  &  OxFFOO)  »  8 ) ; 
setportbit (MCR, 3) ;  //turn  OUT2  on 

BYTE   opt  =  0; 
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if  (parity  !=  NOPARITY)   { 

setbit (opt , 3 ) ;        //  enable  parity 

if  (parity  ==  EVEN)   //  set  even  parity  bit.  if  odd,  leave  bit  0 
setbit (opt, 4) ; 

} 

//  now  set  the  word  length,  len  of  5  sets  both  bits  0  and  1  to 

//  0,  6  sets  to  01,  7  to  10  and  8  to  11 
opt  1=  wordlen-5; 
opt  |=  --stopbits  <<  2 ; 
outportb(LCR, opt ) ; 

if  (ShakeType  ==  RTS_CTS)   { 

setDTRon ( ) ; 

setRTSon ( ) ; 
} 


} 


/**************************************************************** 

PROGRAM:  Get 

AUTHOR : Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Gets  a  byte  from  the  port.  Returns  true  if  there's  one  there,  and 

fills  in  the  byte  parameter.  If  theres  no  character,  the  parameter  is  left  alone, 

and  false  is  returned 
****************************************************** 

Boolean 

serialPort : :Get (BYTE&  data) 

{ 

if  (dataReady ( ) )   {         //make  sure  there's  a  char  there 
data  =  inportb (RX) ;       //read  character  from  8250 
return  TRUE; 

} 
else 

return  FALSE ; 
} 
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PROGRAM:  Send 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION: Outputs  a  single  character  to  the  port.  Returns  Boolean 

status  indicating  whether  successful 

Boolean 

serialPort :: Send (BYTE  data) 

{ 

while  ( ! (ifportbit (LSR, 5) ) )       //  wait  until  THR  ready 
;   //  NULL  statement 

switch  (ShakeType)   ( 
case  NONE: 

outportb(TX, data) ; 
return  TRUE; 
case  RTS_CTS: 

if  (isCTSonO  &&  isDSRonO)   { 
outportb(TX,data) ; 
return  TRUE; 
} 

else   return  FALSE ; 
case  XON_XOFF: 
default : 

//  add  this  later  if  needed 
break; 
} 
return  FALSE; 
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/**************************************************************** 

PROGRAM:  dataReady 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 
DATE: 11  July  1995 

FUNCTION: Checks  port  to  see  if  a  character  has  arrived. 
*********************************************************** 

inline 

Boolean 

ser ialPort : : dataReady ( ) 

{ 

/* 

if  (ifportbit (LSR, 1) )  { 

cerr  «"\nOverrun  Error\n"; 

} 

if  (ifportbit (LSR, 2) )  { 

cerr  <<"\nParity  Error \n " ; 

} 

if  (ifportbit (LSR, 3) )  { 

cerr  <<" \nFraming  Error\n"; 

} 
*/ 

return  ifportbit (LSR, 0 ) ; 
} 

/**************************************************************** 

PROGRAM:  ifportbit 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  19  9  5 

FUNCTION: 
****************************************************************/ 

inline 

Boolean 

serialPort :: ifportbit (WORD  reg,  BYTE  bit) 

{ 

BYTE  on  =  inportb(reg) ; 

on  &=  set (bit ) ; 

return  Boolean (on  ==  set (bit)), - 
} 


144 


/************************************************************•*■*** 

PROGRAM:  toggleDTR 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:  toggles  Data  Transmit  Ready  if  RTS_CTS  is  off 
*************************************************************.***/ 

void 

serialPort : : toggleDTR ( ) 

{ 

if  (ShakeType  !=  RTS_CTS) 

return; 
if  (DTRstate  ==  off) 

setDTRon( ) ; 
else 

setDTRoff ( ) ; 
toggle (DTRstate) ; 
} 

/**************************************************************** 

PROGRAM:  toggleRTS 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:  toggle  Ready  to  Send  (RTS)  if  RTS_CTS  is  on. 
********************************************************* 

void 

serialPort : : toggleRTS ( ) 

{ 

if  (ShakeType  !=  RTS_CTS) 

return; 
if  (RTSstate  ==  off) 

setRTSon( ) ; 
else 

setRTSof f ( ) ; 
toggle (RTSstate) ; 
} 

/**************************************************************** 

PROGRAM:  toggle 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:  toggles  value  of  the  input  variable 
****************************************************************/ 

inline 

void 

serialPort :: toggle (Shake&  h) 

{ 

if  (h  ==  off) 

h  =  on; 
else 

h  =  off; 
} 
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//     buf feredSerialPort 
//*********************■******************• 

PROGRAM : buf feredSerialPort  ( Constructor ) 
AUTHOR : Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 
DATE: 11  July  1995 
FUNCTION: 
Initializes  the  interrupts  for  the  Serial  Port. 

1)  takes  over  the  original  COM  interrupt 

2)  sets  the  port  bits,  parity,  and  stop  bit 

3)  enables  interrupts  on  the  8250  (async  chip) 

4)  enables  the  async  interrupt  on  the  8259  PIC 
it***************************************************************/ 

buf feredSerialPort : :buf feredSerialPort (COMport  portnum,  BYTE  irq, 

BaudRate  baud,  ParityType  parity,  BYTE  wordlen, 
BYTE  stopbits,  handshake  hs,  Buffers  b) 
:  serialPort (portnum,  baud,  parity,  wordlen,  stopbits,  hs), 
buf(b),  irqbit(irq),  comint ( irqbit+8) 

{ 

if  (ShakeType  ==  RTS_CTS)   {  //  turn  it  off  first,  because  it 

//  was  enabled 
setDTRof f ( ) ;  //  in  the  base  class 

setRTSoff ( ) ; 

} 

origcomint  =  getvect (comint ) ;  //remember  the  original  vector 

switch  (portnum)  { 
case  COM1 : 

setvect (comint , COMlhandler) ;  //point  to  the  new  handler 

break; 
case  COM2 : 

setvect (comint , COM2handler) ;  //point  to  the  new  handler 

break; 
case  COM3 : 

setvect (comint , COM3handler) ;  //point  to  the  new  handler 

break; 
case  COM4 : 

setvect (comint , COM4handler) ;  //point  to  the  new  handler 

break; 
} 

//    setportbit (MCR, 3) ;  //turn  0UT2  on 

disable();      //  disable  all  interrupts  -  critical  section 
setportbit ( IER, rx_rdy) ;  //enable  ints  on  receive  only 

origirq  =  inportb ( IRQPORT) ;  //remember  how  it  was 

clrportbit (IRQPORT, irqbit) ;  //enable  COM  ints 

if  (ShakeType  ==  RTS_CTS)   { 
setDTRon ( ) ; 
setRTSon ( ) ; 

} 
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enable ( ) ; 
EOI; 

} 

/************************************************************.*..*..*..*. 

PROGRAM:  -buf f eredSerialPort 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 
DATE: 11  July  1995 
FUNCTION: 
Resets  the  interrupts.   1)  disables  the  8250  (async  chip) 

2)  disables  the  interrupt  chip  for  async  int 

3)  resets  the  8259  PIC 


**************************************************************** 


/ 


buff eredSerialPort : : -buff eredSerialPort ( ) 

{ 

setvect (comint , origcomint ) ;    //set  the  interrupt  vector  back 
outportb(IER, 0) ;  //disable  further  UART  interrupts 

outportb(MCR, 0 ) ;  //turn  everything  off 

outportb(IRQPORT, origirq) ; 
EOI; 

} 

/**************************************************************** 

PROGRAM:  Get 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  199  5 

FUNCTION:   Calls  Get  base  on  buffer  type 
****************************************************************/ 

Boolean 

buf f eredSerialPort : :Get(BYTE&  data) 

{ 

return  buf .Get (data) ; 
} 

/**************************************************************** 

PROGRAM:  processlnterupt 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:   Calls  the  ISR  based  upon  buffer  type 
****************************************************************/ 

void 

buf f eredSerialPort : :processInterrupt ( ) 

{ 

if  (dataReady ( ) )   {         //make  sure  there's  a  char  there 
BYTE  data  =  inportb(RX) ;     //read  character  from  8250 
buf .Add (data) ; 

if  (ShakeType  ==  RTS_CTS  &&  buf . capacityUsed ( )  >  ALMOST_FULL) 
setDTRoff ( ) ; 
} 
} 
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/**************************************************************** 

PROGRAM:  showPorts 

AUTHOR: Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE: 11  July  1995 

FUNCTION:    Prints  interupt  vector  addresses 
********************************************************** 

int 

showPorts ( ) 
{ 

BYTE*  p  =  (BYTE*)C0M2base; 
P  +=  5; 

f print f (stderr, "%X    ",*p++); 
fprintf (stderr, "%X\n" , *p++) ; 
fprintf (stderr, "IRQPORT  =  %X",  inportb { IRQPORT) ) ; 
return  0 ; 
} 
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J.  COMPBUFF.H 

ttifndef  COMPBUFF_H 

#define  COMPBUFF_H 

#include  "buffer.h" 
#include  "toetypes.h" 

#define  COMPBLOCKS  8 
#define  LINE_FEED  10 
#define   CARR  RETURN     13 


/ 


************************************************************************* 

Class  buffers  COMPASS  messages  received  via  serial  port  communications. 
Uses  a  multiple  buffer  system  in  which  each  buffer  is  capable  of 
holding  a  single  message.  Buffers  are  filled  and  processed 
sequentially  in  a  round  robin  fashion.  Messages  are  checked  for  validity 
only  upon  attempted  reads  from  the  buffer. 

***************************************************** 

class  compBuffer  :  public  Buffer   { 

public : 

compBuffer (BYTE  compBlocks=COMPBLOCKS) ; 
-compBuff er () {delete  []  block;} 

Boolean  hasData ( ) ;      //  a  complete  structure  is  ready 

Boolean  Get(BYTE&)    {return  FALSE;}  //Satisfy  inheritence  requirements 

Boolean  Get (compData) ;   //  get  a  complete  structure  filled  in 

void  Add(BYTE  ch) ;   //  build  the  structure  as  each  byte  is  added 

protected : 

Boolean   validHeader (compData) ;  //  check  a  block  for  valid  header 
compData  *block;      //  Pointer  to  array  of  compass  messages 
WORD     current,     //  the  current  comp  block  in  use 
last;        //  the  last  comp  block  in  use 

BYTE      *putPlace;   //  place  to  put  the  next  charater  received 
}; 

#endif 
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K.  COMPBUFF.CPP 

#include  <iostream.h> 
#include  <stdio.h> 

# include  "compbuf f .h" 

/*********************************************************************** 

PROGRAM : compBuf fer  (Constructor) 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 28  April  1996 

FUNCTION: 

Allocates  message  buffers,  indicates  that  no  data  has  been 

received  by  equalizing  current  and  last  and  sets  the  position 

into  which  initial  character  will  be  read. 

RETURNS: nothing. 

CALLED  BY: compass  class  ( compass. h) 

CALLS : none . 
****************************************************** 

compBuf f er : : compBuf f er (BYTE  compBlocks) : 

current (0) ,  last (0) , 

Buff er (compBlocks)  //Call  to  constructor  of  the  base  class 
{ 

block  =  new  compData [compBlocks] ;  //  Create  an  array  of  message  buffers 

putPlace  =  & (block [current ][ 0 ]) ;  //  Set  position  for  first  character 
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/**********************************************************•*•*•*•■*■**•*.**.*..*..*■.*■ 

PROGRAM: compBuffer: :Add 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE:28  April  1996 

FUNCTION: 

Interrupt  driven  routine  which  writes  incoming  characters 

into  the  coompass  message  buffers 

RETURNS: nothing. 

CALLED  BY:   interupt  driven  by  buf f eredSerialPort 

CALLS : none . 
************************************************ 

void 

compBuf fer: : Add (BYTE  data) { 

static  Boolean  IfFlag  -   FALSE;  //True,  if  message  end  detected 
static  int  messageCount ( 0 ) ;  //  Counts  characters  in  current  message 

//Is  a  new  message  starting? 
if  (IfFlag  &&  (data  ==  '$'))  { 

last  =  current;   //  Set  last  to  buffer  with  newest  message, 
inc (current ) ;     //  Set  current  to  the  next  buffer 

//  Set  putPlace  to  the  beginning  of  the  next  buffer. 
putPlace  =  & (block [current ][ 0 ]) ; 

IfFlag  =  FALSE;   //  reset  for  end  of  next  message. 
} 

*putPlace++  =  data;//  Write  character  into  the  buffer. 
messageCount++ ; 

//Has  the  end  of  a  message  been  received  (<crxlf>)? 
if  (data  ==  LINE_FEED)  { 

IfFlag  =  TRUE; 
} 
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/*********************************************************************** 

PROGRAM rcompBuffer:  -.Get 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 28  April  1996 

FUNCTION: 

Checks  to  see  if  a  new  message  has  arrived,  copies  it  into  the 

input  argument  data  and  returns  a  flag  to  indicate  whether  a  new 

message  was  received. 
RETURNS : TRUE ,  if  a  new  valid  position  has  been  received. 

FALSE,  otherwise 
CALLED  BY:  compass. cpp 
CALLS :compBuffer : :hasData 

****************************************************** 

Boolean 

compBuf fer : :Get (GPSdata  data) 

{ 

//  Has  a  new  valid  message  been  received, 
if  (hasDataO)   { 

//  Copy  the  message  out  of  the  buffer, 
memcpy  (data,  block  +  last,  COMPSIZE) ; 

//  Indicate  that  this  message  has  been  read, 
last  =  current; 
return  TRUE; 
} 
else  { 

return  FALSE; 
} 
} 


152 


/*********************************************************************** 

PROGRAM :compBuf fer : :hasData 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE:28  April  1996 

FUNCTION: 

Determines  whether  a  new  message  has  been  received  and  checks 

to  see  if  it  has  a  valid  header. 

RETURNS : TRUE ,  if  a  new  valid  message  has  been  received. 

CALLED  BY:    compBuf f er : :Get 

CALLS:        validHeader  (compBuf f er . cpp) 
*************************************************** 

Boolean 

compBuf fer: :hasData() 

{ 

if  ((last  !=  current)  &&  (validHeader (block [last ])) )  { 
return  TRUE; 

} 
else  { 

return  FALSE; 
} 

} 
/*********************************************************************** 

PROGRAM : val idHeader 

AUTHOR: Eric  Bachmann,  Randy  Walker 

DATE: 15  May  199  6 

FUNCTION: 

Checks  to  see  if  a  message  has  the  proper  header  for  a  compass 

message.  ($C) 

RETURNS : TRUE ,  if  the  header  is  valid.  FALSE,  otherwise. 

CALLED  BY:compBuf fer : :hasData 

CALLS: none. 

COMMENTS : 
*************************************************************************/ 

Boolean 

compBuf fer: : validHeader (compData  dataPtr) 

{ 

if  ((dataPtr[0]  ==  '$')  && 
(dataPtr[l]  ==  'C'))  { 
return  TRUE; 
} 
else  { 

return  FALSE; 
} 
} 
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